Add flex angle meas. + est. of required angle

This commit is contained in:
Thomas Dehaeze 2020-08-05 16:15:37 +02:00
parent ebb928b890
commit 09dad1482c
6 changed files with 195 additions and 16 deletions

Binary file not shown.

View File

@ -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]
Ty_max = 50e-6; % Translation [m]
Tz_max = 50e-6; % Translation [m]
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: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);
stewart = computeJointsPose(stewart);
Ry = [ cos(Ps(Ps_i, 5)) 0 sin(Ps(Ps_i, 5));
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>>

View File

@ -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

View File

@ -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)';