tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Test of position/orientation of an Hexapod

Browse Source
This commit is contained in:
Thomas Dehaeze
2019-12-10 18:06:33 +01:00
parent b14de47379
commit 9fb86964ef
7 changed files with 819 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/initializeMicroHexapod.m
View File

@@ -11,7 +11,11 @@
function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Default values for opts
opts = struct('rigid', false);
opts = struct(...
'rigid', false, ...
'AP', zeros(3, 1), ... % Wanted position in [m] of OB with respect to frame {A}
'ARB', eye(3) ... % Rotation Matrix that represent the wanted orientation of frame {B} with respect to frame {A}
);
%% Populate opts with input parameters
if exist('opts_param','var')
@@ -23,7 +27,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
%% Stewart Object
micro_hexapod = struct();
micro_hexapod.h = 350; % Total height of the platform [mm]
micro_hexapod.jacobian = 270; % Distance from the top platform to the Jacobian point [mm]
micro_hexapod.jacobian = 270; % Distance from the top of the mobile platform to the Jacobian point [mm]
%% Bottom Plate - Mechanical Design
BP = struct();
@@ -58,7 +62,7 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
else
Leg.k.ax = 2e7; % Stiffness of each leg [N/m]
end
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
Leg.ksi.ax = 0.1; % Modal damping ksi = 1/2*c/sqrt(km) []
Leg.rad.bottom = 25; % Radius of the cylinder of the bottom part [mm]
Leg.rad.top = 17; % Radius of the cylinder of the top part [mm]
Leg.density = 8000; % Density of the material [kg/m^3]
@@ -104,6 +108,9 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
%%
micro_hexapod = initializeParameters(micro_hexapod);
%% Setup equilibrium position of each leg
micro_hexapod.L0 = initializeHexapodPosition(micro_hexapod, opts.AP, opts.ARB);
%% Save
save('./mat/stages.mat', 'micro_hexapod', '-append');
@@ -197,11 +204,34 @@ function [micro_hexapod] = initializeMicroHexapod(opts_param)
stewart.J = getJacobianMatrix(leg_vectors', aa');
end
function J = getJacobianMatrix(RM,M_pos_base)
%%
function J = getJacobianMatrix(RM, M_pos_base)
% RM: [3x6] unit vector of each leg in the fixed frame
% M_pos_base: [3x6] vector of the leg connection at the top platform location in the fixed frame
J = zeros(6);
J(:, 1:3) = RM';
J(:, 4:6) = cross(M_pos_base, RM)';
end
%%
function [L] = initializeHexapodPosition(hexapod, AP, ARB)
% initializeHexapodPosition - Compute the initial position of each leg to have the wanted Hexapod's position
%
% Syntax: initializeHexapodPosition(hexapod, AP, ARB)
%
% Inputs:
% - hexapod - Hexapod object containing the geometry of the hexapod
% - AP - Position vector of point OB expressed in frame {A} in [m]
% - ARB - Rotation Matrix expressed in frame {A}
% Wanted Length of the hexapod's legs [m]
L = zeros(6, 1);
for i = 1:length(L)
Bbi = hexapod.pos_top_tranform(i, :)' - 1e-3*[0 ; 0 ; hexapod.TP.thickness+hexapod.Leg.sphere.top+hexapod.SP.thickness.top+hexapod.jacobian]; % [m]
Aai = hexapod.pos_base(i, :)' + 1e-3*[0 ; 0 ; hexapod.BP.thickness+hexapod.Leg.sphere.bottom+hexapod.SP.thickness.bottom-micro_hexapod.h-hexapod.jacobian]; % [m]
L(i) = sqrt(AP'*AP + Bbi'*Bbi + Aai'*Aai - 2*AP'*Aai + 2*AP'*(ARB*Bbi) - 2*(ARB*Bbi)'*Aai);
end
end
end