Update the way the micro-hexapod is initialize (pitch-roll-yaw)

Next verified that the function to compute the wanted sample position
is working => yes

simscape/index.org

@@ -755,8 +755,7 @@ This Matlab function is accessible [[file:../src/runSimulation.m][here]].
           'time', 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz', 'K');
   end
 #+end_src
-* Initialize Elements
-<<sec:initialize_elements>>
+* Helping Functions
 ** Experiment
 :PROPERTIES:
 :header-args:matlab+: :tangle ../src/initializeExperiment.m
@@ -797,7 +796,6 @@ This Matlab function is accessible [[file:../src/initializeExperiment.m][here]].
 :header-args:matlab+: :comments org :mkdirp yes
 :header-args:matlab+: :eval no :results none
 :END:
-
 <<sec:initializeReferences>>
 
 This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
@@ -817,7 +815,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
           'Rz_amplitude', 0, ...          % Initial angle [rad]
           'Rz_period',    1, ...          % Period of the rotating [s]
           'Dh_type',      'constant', ... % For now, only constant is implemented
-          'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform
+          'Dh_pos',       [0; 0; 0; 0; 0; 0], ...  % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
           'Rm_type',      'constant', ... % For now, only constant is implemented
           'Rm_pos',       [0; pi],  ...   % Initial position of the two masses
           'Dn_type',      'constant', ... % For now, only constant is implemented
@@ -887,14 +885,37 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
       %% Micro-Hexapod
       t = [0, Ts];
       Dh = zeros(length(t), 6);
+      Dhl = zeros(length(t), 6);
 
       switch opts.Dh_type
         case 'constant'
           Dh = [opts.Dh_pos, opts.Dh_pos];
+
+          load('./mat/stages.mat', 'micro_hexapod');
+
+          AP = [opts.Dh_pos(1) ; opts.Dh_pos(2) ; opts.Dh_pos(3)];
+
+          tx = opts.Dh_pos(4);
+          ty = opts.Dh_pos(5);
+          tz = opts.Dh_pos(6);
+
+          ARB = [cos(tz) -sin(tz) 0;
+                 sin(tz)  cos(tz) 0;
+                 0        0       1]*...
+                [ cos(ty) 0 sin(ty);
+                 0        1 0;
+                 -sin(ty) 0 cos(ty)]*...
+                [1 0        0;
+                 0 cos(tx) -sin(tx);
+                 0 sin(tx)  cos(tx)];
+
+          [Dhl] = inverseKinematicsHexapod(micro_hexapod, AP, ARB);
+          Dhl = [Dhl, Dhl];
         otherwise
           warning('Dh_type is not set correctly');
       end
       Dh = struct('time', t, 'signals', struct('values', Dh));
+      Dhl = struct('time', t, 'signals', struct('values', Dhl));
 
       %% Axis Compensation - Rm
       t = [0, Ts];
@@ -914,7 +935,7 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
       Dn = struct('time', t, 'signals', struct('values', Dn));
 
       %% Save
-      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Rm', 'Dn', 'Ts');
+      save('./mat/nass_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'Rm', 'Dn', 'Ts');
   end
 #+end_src
 
@@ -1108,6 +1129,42 @@ This Matlab function is accessible [[file:../src/initializeInputs.m][here]].
   end
 #+end_src
 
+** Inverse Kinematics of the Hexapod
+:PROPERTIES:
+:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
+:header-args:matlab+: :comments org :mkdirp yes
+:header-args:matlab+: :eval no :results none
+:END:
+<<sec:inverseKinematicsHexapod>>
+
+This Matlab function is accessible [[file:src/inverseKinematicsHexapod.m][here]].
+
+#+begin_src matlab
+  function [L] = inverseKinematicsHexapod(hexapod, AP, ARB)
+  % inverseKinematicsHexapod - Compute the initial position of each leg to have the wanted Hexapod's position
+  %
+  % Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
+  %
+  % Inputs:
+  %    - hexapod - Hexapod object containing the geometry of the hexapod
+  %    - AP - Position vector of point OB expressed in frame {A} in [m]
+  %    - ARB - Rotation Matrix expressed in frame {A}
+
+    % Wanted Length of the hexapod's legs [m]
+    L = zeros(6, 1);
+
+    for i = 1:length(L)
+      Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
+      Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-hexapod.h-hexapod.jacobian]; % [m]
+
+      L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
+    end
+  end
+#+end_src
+
+
+* Initialize Elements
+<<sec:initialize_elements>>
 ** Ground
 :PROPERTIES:
 :header-args:matlab+: :tangle ../src/initializeGround.m
@@ -1420,19 +1477,19 @@ This Matlab function is accessible [[file:../src/initializeRz.m][here]].
 
 ** Initialize Hexapod legs' length
 :PROPERTIES:
-:header-args:matlab+: :tangle ../src/initializeHexapodPosition.m
+:header-args:matlab+: :tangle ../src/inverseKinematicsHexapod.m
 :header-args:matlab+: :comments org :mkdirp yes
 :header-args:matlab+: :eval no :results none
 :END:
-<<sec:initializeHexapodPosition>>
+<<sec:inverseKinematicsHexapod>>
 
-This Matlab function is accessible [[file:../src/initializeHexapodPosition.m][here]].
+This Matlab function is accessible [[file:../src/inverseKinematicsHexapod.m][here]].
 
 #+begin_src matlab
-  function [hexapod] = initializeHexapodPosition(hexapod, AP, ARB)
-  % initializeHexapodPosition -
+  function [hexapod] = inverseKinematicsHexapod(hexapod, AP, ARB)
+  % inverseKinematicsHexapod -
   %
-  % Syntax: initializeHexapodPosition(hexapod, AP, ARB)
+  % Syntax: inverseKinematicsHexapod(hexapod, AP, ARB)
   %
   % Inputs:
   %    - hexapod - Hexapod object containing the geometry of the hexapod
@@ -1563,7 +1620,7 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
       micro_hexapod = initializeParameters(micro_hexapod);
 
       %% Setup equilibrium position of each leg
-      micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
+      micro_hexapod.L0 = inverseKinematicsHexapod(micro_hexapod, opts.AP, opts.ARB);
 
       %% Save
       save('./mat/stages.mat', 'micro_hexapod', '-append');
@@ -1666,28 +1723,6 @@ This Matlab function is accessible [[file:../src/initializeMicroHexapod.m][here]
           J(:, 1:3) = RM';
           J(:, 4:6) = cross(M_pos_base, RM)';
       end
-
-      %%
-      function [L] = initializeHexapodPosition(hexapod, AP, ARB)
-      % initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
-      %
-      % Syntax: initializeHexapodPosition(hexapod, AP, ARB)
-      %
-      % Inputs:
-      %    - hexapod - Hexapod object containing the geometry of the hexapod
-      %    - AP - Position vector of point OB expressed in frame {A} in [m]
-      %    - ARB - Rotation Matrix expressed in frame {A}
-
-        % Wanted Length of the hexapod's legs [m]
-        L = zeros(6, 1);
-
-        for i = 1:length(L)
-          Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
-          Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
-
-          L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
-        end
-      end
   end
 #+end_src