Add flex angle meas. + est. of required angle

org/kinematic-study.org

@@ -615,7 +615,6 @@ using this function https://fr.mathworks.com/help/matlab/ref/contour3.html
 
 * Estimation of the Joint required Stroke
 ** Introduction                                                      :ignore:
-
 ** 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>>
@@ -629,37 +628,62 @@ using this function https://fr.mathworks.com/help/matlab/ref/contour3.html
   simulinkproject('../');
 #+end_src
 
-** Example of the initialization of a Stewart Platform
+** Estimation with an analytical model
 Let's first define the Stewart Platform Geometry.
 #+begin_src matlab
   stewart = initializeStewartPlatform();
   stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 150e-3);
   stewart = generateGeneralConfiguration(stewart);
-  stewart = computeJointsPose(stewart);
+  stewart = computeJointsPose(stewart, 'AP', [0; 0; 0]);
 
   As_init = stewart.geometry.As;
 #+end_src
 
 #+begin_src matlab
-  Tx_max = 50e-6; % Translation [m]
+  Tx_max = 60e-6; % Translation [m]
-  Ty_max = 50e-6; % Translation [m]
+  Ty_max = 60e-6; % Translation [m]
-  Tz_max = 50e-6; % Translation [m]
+  Tz_max = 60e-6; % Translation [m]
+  Rx_max = 30e-6; % Rotation [rad]
+  Ry_max = 30e-6; % Rotation [rad]
+  Rz_max = 0; % Rotation [rad]
 #+end_src
 
 #+begin_src matlab
-  Ps = [2*(dec2bin(0:3^2-2,3)-'0')-1].*[Tx_max Ty_max Tz_max];
+  Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
 #+end_src
 
 #+begin_src matlab
   flex_ang = zeros(size(Ps, 1), 6);
+  Rxs = zeros(size(Ps, 1), 6)
+  Rys = zeros(size(Ps, 1), 6)
+  Rzs = zeros(size(Ps, 1), 6)
 
   for Ps_i = 1:size(Ps, 1)
-      stewart.geometry.FO_M = [0; 0; 90e-3] + Ps(Ps_i, :)';
+      Rx = [1 0        0;
+            0 cos(Ps(Ps_i, 4)) -sin(Ps(Ps_i, 4));
+            0 sin(Ps(Ps_i, 4))  cos(Ps(Ps_i, 4))];
 
-      stewart = generateGeneralConfiguration(stewart);
+      Ry = [ cos(Ps(Ps_i, 5)) 0 sin(Ps(Ps_i, 5));
-      stewart = computeJointsPose(stewart);
+             0        1 0;
+             -sin(Ps(Ps_i, 5)) 0 cos(Ps(Ps_i, 5))];
+
+      Rz = [cos(Ps(Ps_i, 6)) -sin(Ps(Ps_i, 6)) 0;
+            sin(Ps(Ps_i, 6))  cos(Ps(Ps_i, 6)) 0;
+            0        0       1];
+
+      ARB = Rz*Ry*Rx;
+
+      stewart = computeJointsPose(stewart, 'AP', Ps(Ps_i, 1:3)', 'ARB', ARB);
 
       flex_ang(Ps_i, :) = acos(sum(As_init.*stewart.geometry.As));
+     
+      for l_i = 1:6
+          MRf = stewart.platform_M.MRb(:,:,l_i)*(ARB')*(stewart.platform_F.FRa(:,:,l_i)');
+         
+          Rys(Ps_i, l_i) = atan2(MRf(1,3), sqrt(MRf(1,1)^2 + MRf(2,2)^2));
+          Rxs(Ps_i, l_i) = atan2(-MRf(2,3)/cos(Rys(Ps_i, l_i)), MRf(3,3)/cos(Rys(Ps_i, l_i)));
+          Rzs(Ps_i, l_i) = atan2(-MRf(1,2)/cos(Rys(Ps_i, l_i)), MRf(1,1)/cos(Rys(Ps_i, l_i)));
+      end
   end
 #+end_src
 
@@ -669,7 +693,126 @@ And the maximum bending of the flexible joints is: (in [mrad])
 #+end_src
 
 #+RESULTS:
-: 0.90937
+: 1.1704
+
+#+begin_src matlab :results replace value
+  1e3*max(max(sqrt(Rxs.^2 + Rys.^2)))
+#+end_src
+
+#+RESULTS:
+: 0.075063
+
+#+begin_src matlab :results replace value
+  1e3*max(max(Rzs))
+#+end_src
+
+#+RESULTS:
+: 0.04666
+
+** Estimation using the Simscape Model
+*** Model Init
+#+begin_src matlab
+  open('stewart_platform_model.slx')
+#+end_src
+
+#+begin_src matlab
+  stewart = initializeStewartPlatform();
+  stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
+  stewart = generateGeneralConfiguration(stewart);
+  stewart = computeJointsPose(stewart);
+  stewart = initializeStrutDynamics(stewart);
+  stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
+  stewart = initializeCylindricalPlatforms(stewart);
+  stewart = initializeCylindricalStruts(stewart);
+  stewart = computeJacobian(stewart);
+  stewart = initializeStewartPose(stewart);
+  stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
+#+end_src
+
+#+begin_src matlab
+  ground = initializeGround('type', 'rigid');
+  payload = initializePayload('type', 'none');
+  controller = initializeController('type', 'open-loop');
+  disturbances = initializeDisturbances();
+  references = initializeReferences(stewart);
+#+end_src
+
+*** Controller design to be close to the wanted displacement
+#+begin_src matlab
+  %% Name of the Simulink File
+  mdl = 'stewart_platform_model';
+
+  %% Input/Output definition
+  clear io; io_i = 1;
+  io(io_i) = linio([mdl, '/Controller'],        1, 'openinput');  io_i = io_i + 1; % Actuator Force Inputs [N]
+  io(io_i) = linio([mdl, '/Stewart Platform'],  1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
+
+  %% Run the linearization
+  G = linearize(mdl, io);
+  G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
+  G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
+#+end_src
+
+#+begin_src matlab
+  wc = 2*pi*30;
+  Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/wc);
+#+end_src
+
+#+begin_src matlab
+  controller = initializeController('type', 'ref-track-L');
+#+end_src
+
+*** Simulations
+#+begin_src matlab
+  Tx_max = 60e-6; % Translation [m]
+  Ty_max = 60e-6; % Translation [m]
+  Tz_max = 60e-6; % Translation [m]
+  Rx_max = 30e-6; % Rotation [rad]
+  Ry_max = 30e-6; % Rotation [rad]
+  Rz_max = 0; % Rotation [rad]
+#+end_src
+
+#+begin_src matlab
+  Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
+#+end_src
+
+#+begin_src matlab
+  cl_perf = zeros(size(Ps, 1), 1); % Closed loop performance [percentage]
+  Rs = zeros(size(Ps, 1), 5); % Max Flexor angles for the 6 legs [mrad]
+
+  for Ps_i = 1:size(Ps, 1)
+      fprintf('Experiment %i over %i', Ps_i, size(Ps, 1));
+     
+      references = initializeReferences(stewart, 't', 0, 'r', Ps(Ps_i, :)');
+      set_param('stewart_platform_model','StopTime','0.1');
+      sim('stewart_platform_model');
+
+      cl_perf(Ps_i) = 100*max(abs((simout.y.dLm.Data(end, :) - references.rL.Data(:,1,1)')./simout.y.dLm.Data(end, :)));
+      Rs(Ps_i, :) = [max(abs(1e3*simout.y.Rf.Data(end, 1:2:end))), max(abs(1e3*simout.y.Rf.Data(end, 2:2:end))), max(abs(1e3*simout.y.Rm.Data(end, 1:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 2:3:end))), max(abs(1e3*simout.y.Rm.Data(end, 3:3:end)))]';
+  end
+#+end_src
+
+Verify that the simulations are all correct:
+#+begin_src matlab :results replace value
+  max(cl_perf)
+#+end_src
+
+#+RESULTS:
+: 8.1147
+
+*** Results
+#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
+  data2orgtable(max(Rs)', {'Rx bot', 'Ry bot', 'Rx top', 'Ry top', 'Rz top'}, {'Stroke [mrad]'}, ' %.2f ');
+#+end_src
+
+#+RESULTS:
+|        | Stroke [mrad] |
+|--------+---------------|
+| Rx bot |          1.03 |
+| Ry bot |          0.93 |
+| Rx top |          1.06 |
+| Ry top |          0.95 |
+| Rz top |          0.03 |
 
 * Functions
 <<sec:functions>>

org/stewart-architecture.org

@@ -632,10 +632,10 @@ This Matlab function is accessible [[file:../src/computeJointsPose.m][here]].
 :UNNUMBERED: t
 :END:
 #+begin_src matlab
-  function [stewart] = computeJointsPose(stewart)
+  function [stewart] = computeJointsPose(stewart, args)
   % computeJointsPose -
   %
-  % Syntax: [stewart] = computeJointsPose(stewart)
+  % Syntax: [stewart] = computeJointsPose(stewart, args)
   %
   % Inputs:
   %    - stewart - A structure with the following fields
@@ -644,6 +644,9 @@ This Matlab function is accessible [[file:../src/computeJointsPose.m][here]].
   %        - 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}
+  %    - 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 - A structure with the following added fields
@@ -660,6 +663,18 @@ This Matlab function is accessible [[file:../src/computeJointsPose.m][here]].
   %        - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
 #+end_src
 
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+  end
+#+end_src
+
 *** Check the =stewart= structure elements
 :PROPERTIES:
 :UNNUMBERED: t
@@ -688,11 +703,20 @@ This Matlab function is accessible [[file:../src/computeJointsPose.m][here]].
 #+begin_src matlab
   Aa = Fa - repmat(FO_A, [1, 6]);
   Bb = Mb - repmat(MO_B, [1, 6]);
+#+end_src
 
+Original:
+#+begin_src matlab
   Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
   Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
 #+end_src
 
+Translation & Rotation: (Rotation and then translation)
+#+begin_src matlab
+  Ab = args.ARB *Bb - repmat(-args.AP, [1, 6]);
+  Ba = args.ARB'*Aa - repmat( args.AP, [1, 6]);
+#+end_src
+
 *** Compute the strut length and orientation
 :PROPERTIES:
 :UNNUMBERED: t

src/computeJointsPose.m

@@ -1,7 +1,7 @@
-function [stewart] = computeJointsPose(stewart)
+function [stewart] = computeJointsPose(stewart, args)
 % computeJointsPose -
 %
-% Syntax: [stewart] = computeJointsPose(stewart)
+% Syntax: [stewart] = computeJointsPose(stewart, args)
 %
 % Inputs:
 %    - stewart - A structure with the following fields
@@ -10,6 +10,9 @@ function [stewart] = computeJointsPose(stewart)
 %        - 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}
+%    - 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 - A structure with the following added fields
@@ -25,6 +28,12 @@ function [stewart] = computeJointsPose(stewart)
 %        - 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}
 
+arguments
+    stewart
+    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+    args.ARB (3,3) double {mustBeNumeric} = eye(3)
+end
+
 assert(isfield(stewart.platform_F, 'Fa'),   'stewart.platform_F should have attribute Fa')
 Fa = stewart.platform_F.Fa;
 
@@ -46,6 +55,9 @@ 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]);
 
+Ab = args.ARB *(Bb - repmat(-args.AP, [1, 6]));
+Ba = args.ARB'*(Aa - repmat( args.AP, [1, 6]));
+
 As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
 
 l = vecnorm(Ab - Aa)';