Add cubic configuration initialization
This commit is contained in:
parent
f902c6f308
commit
c69f79bf5d
@ -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.
Loading…
Reference in New Issue
Block a user