Add cubic configuration initialization

This commit is contained in:
Thomas Dehaeze 2019-12-20 11:57:39 +01:00
parent f902c6f308
commit c69f79bf5d
2 changed files with 230 additions and 4 deletions

View File

@ -63,27 +63,50 @@ For Simscape, we need:
- The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$ - The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$
------ * Procedure
The procedure is the following: The procedure is the following:
1. Choose $H$ 1. Choose $H$
2. Choose ${}^{F}\bm{O}_{A}$ and ${}^{M}\bm{O}_{B}$ 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}$ 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}$ 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 #+begin_src matlab
%% 1. Height of the platform. Location of {F} and {M} %% 1. Height of the platform. Location of {F} and {M}
H = 90e-3; % [m] H = 90e-3; % [m]
FO_M = [0; 0; H]; FO_M = [0; 0; H];
#+end_src
** Define the location of {A} and {B}
#+begin_src matlab
%% 2. Location of {A} and {B} %% 2. Location of {A} and {B}
FO_A = [0; 0; 100e-3] + FO_M;% [m,m,m] FO_A = [0; 0; 100e-3] + FO_M;% [m,m,m]
MO_B = [0; 0; 100e-3];% [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 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 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]); Aa = Fa - repmat(FO_A, [1, 6]);
Bb = Mb - repmat(MO_B, [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) = [cross([0;1;0],Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
end end
#+end_src
** Define the dynamical properties of each strut
#+begin_src matlab
%% 4. Stiffness and Damping of each strut %% 4. Stiffness and Damping of each strut
Ki = 1e6*ones(6,1); Ki = 1e6*ones(6,1);
Ci = 1e2*ones(6,1); Ci = 1e2*ones(6,1);
#+end_src #+end_src
------ ** Old Introduction :ignore:
First, geometrical parameters are defined: First, geometrical parameters are defined:
- ${}^A\bm{a}_i$ - Position of the joints fixed to the fixed base w.r.t $\{A\}$ - ${}^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 point for the differential measurements
- Location of the Jacobian point for velocity/displacement computation - 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 * initializeGeneralConfiguration
:PROPERTIES: :PROPERTIES:
:HEADER-ARGS:matlab+: :exports code :HEADER-ARGS:matlab+: :exports code

Binary file not shown.