Add flex angle meas. + est. of required angle
This commit is contained in:
parent
ebb928b890
commit
09dad1482c
Binary file not shown.
@ -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>>
|
||||
|
||||
@ -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
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -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)';
|
||||
|
||||
Loading…
Reference in New Issue
Block a user