[BRK] Update the nano-hexapod Simscape model
This commit is contained in:
Binary file not shown.
Binary file not shown.
@@ -1131,208 +1131,68 @@ This Matlab function is accessible [[file:../src/initializeMirror.m][here]].
|
||||
This Matlab function is accessible [[file:../src/initializeNanoHexapod.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [nano_hexapod] = initializeNanoHexapod(args)
|
||||
arguments
|
||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
function [nano_hexapod] = initializeNanoHexapod(args)
|
||||
arguments
|
||||
% initializeFramesPositions
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 175e-3
|
||||
% generateGeneralConfiguration
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 100e-3;
|
||||
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
||||
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
% initializeStrutDynamics
|
||||
args.actuator char {mustBeMember(args.actuator,{'piezo', 'lorentz'})} = 'piezo'
|
||||
% initializeCylindricalPlatforms
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
||||
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
||||
% initializeCylindricalStruts
|
||||
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
||||
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
||||
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
||||
% inverseKinematics
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
%% Stewart Object
|
||||
nano_hexapod = struct();
|
||||
nano_hexapod.h = 90; % Total height of the platform [mm]
|
||||
nano_hexapod.jacobian = 175; % Point where the Jacobian is computed => Center of rotation [mm]
|
||||
stewart = initializeFramesPositions('H', args.H, 'MO_B', args.MO_B);
|
||||
|
||||
%% Bottom Plate
|
||||
BP = struct();
|
||||
stewart = generateGeneralConfiguration(stewart, 'FH', args.FH, 'FR', args.FR, 'FTh', args.FTh, 'MH', args.MH, 'MR', args.MR, 'MTh', args.MTh);
|
||||
|
||||
BP.rad.int = 0; % Internal Radius [mm]
|
||||
BP.rad.ext = 150; % External Radius [mm]
|
||||
BP.thickness = 10; % Thickness [mm]
|
||||
BP.leg.rad = 100; % Radius where the legs articulations are positionned [mm]
|
||||
BP.leg.ang = 5; % Angle Offset [deg]
|
||||
BP.density = 8000;% Density of the material [kg/m^3]
|
||||
BP.color = [0.7 0.7 0.7]; % Color [rgb]
|
||||
BP.shape = [BP.rad.int BP.thickness; BP.rad.int 0; BP.rad.ext 0; BP.rad.ext BP.thickness];
|
||||
stewart = computeJointsPose(stewart);
|
||||
|
||||
%% Top Plate
|
||||
TP = struct();
|
||||
if strcmp(args.actuator, 'piezo')
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e7*ones(6,1), ...
|
||||
'Ci', 1e2*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'lorentz')
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e4*ones(6,1), ...
|
||||
'Ci', 1e2*ones(6,1));
|
||||
else
|
||||
error('args.actuator should be piezo or lorentz');
|
||||
end
|
||||
|
||||
TP.rad.int = 0; % Internal Radius [mm]
|
||||
TP.rad.ext = 100; % Internal Radius [mm]
|
||||
TP.thickness = 10; % Thickness [mm]
|
||||
TP.leg.rad = 90; % Radius where the legs articulations are positionned [mm]
|
||||
TP.leg.ang = 5; % Angle Offset [deg]
|
||||
TP.density = 8000;% Density of the material [kg/m^3]
|
||||
TP.color = [0.7 0.7 0.7]; % Color [rgb]
|
||||
TP.shape = [TP.rad.int TP.thickness; TP.rad.int 0; TP.rad.ext 0; TP.rad.ext TP.thickness];
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
|
||||
%% Leg
|
||||
Leg = struct();
|
||||
stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
|
||||
Leg.stroke = 80e-6; % Maximum Stroke of each leg [m]
|
||||
if strcmp(args.actuator, 'piezo')
|
||||
Leg.k.ax = 1e7; % Stiffness of each leg [N/m]
|
||||
elseif strcmp(args.actuator, 'lorentz')
|
||||
Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
|
||||
else
|
||||
error('args.actuator should be piezo or lorentz');
|
||||
end
|
||||
Leg.ksi.ax = 10; % Maximum amplification at resonance []
|
||||
Leg.rad.bottom = 12; % Radius of the cylinder of the bottom part [mm]
|
||||
Leg.rad.top = 10; % Radius of the cylinder of the top part [mm]
|
||||
Leg.density = 8000; % Density of the material [kg/m^3]
|
||||
Leg.color.bottom = [0.5 0.5 0.5]; % Color [rgb]
|
||||
Leg.color.top = [0.5 0.5 0.5]; % Color [rgb]
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
Leg.sphere.bottom = Leg.rad.bottom; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.sphere.top = Leg.rad.top; % Size of the sphere at the end of the leg [mm]
|
||||
Leg.m = TP.density*((pi*(TP.rad.ext/1000)^2)*(TP.thickness/1000)-(pi*(TP.rad.int/1000^2))*(TP.thickness/1000))/6; % TODO [kg]
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||||
|
||||
Leg = updateDamping(Leg);
|
||||
nano_hexapod = stewart;
|
||||
|
||||
|
||||
%% Sphere
|
||||
SP = struct();
|
||||
|
||||
SP.height.bottom = 15; % [mm]
|
||||
SP.height.top = 15; % [mm]
|
||||
SP.density.bottom = 8000; % [kg/m^3]
|
||||
SP.density.top = 8000; % [kg/m^3]
|
||||
SP.color.bottom = [0.7 0.7 0.7]; % [rgb]
|
||||
SP.color.top = [0.7 0.7 0.7]; % [rgb]
|
||||
SP.k.ax = 0; % [N*m/deg]
|
||||
SP.ksi.ax = 0;
|
||||
|
||||
SP.thickness.bottom = SP.height.bottom-Leg.sphere.bottom; % [mm]
|
||||
SP.thickness.top = SP.height.top-Leg.sphere.top; % [mm]
|
||||
SP.rad.bottom = Leg.sphere.bottom; % [mm]
|
||||
SP.rad.top = Leg.sphere.top; % [mm]
|
||||
SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); % TODO [kg]
|
||||
|
||||
SP = updateDamping(SP);
|
||||
|
||||
%%
|
||||
Leg.support.bottom = [0 SP.thickness.bottom; 0 0; SP.rad.bottom 0; SP.rad.bottom SP.height.bottom];
|
||||
Leg.support.top = [0 SP.thickness.top; 0 0; SP.rad.top 0; SP.rad.top SP.height.top];
|
||||
|
||||
%%
|
||||
nano_hexapod.BP = BP;
|
||||
nano_hexapod.TP = TP;
|
||||
nano_hexapod.Leg = Leg;
|
||||
nano_hexapod.SP = SP;
|
||||
|
||||
%%
|
||||
nano_hexapod = initializeParameters(nano_hexapod);
|
||||
|
||||
%% Setup equilibrium position of each leg
|
||||
nano_hexapod.L0 = inverseKinematicsHexapod(nano_hexapod, args.AP, args.ARB);
|
||||
|
||||
%% Save
|
||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||
|
||||
%%
|
||||
function [element] = updateDamping(element)
|
||||
field = fieldnames(element.k);
|
||||
for i = 1:length(field)
|
||||
if element.ksi.(field{i}) > 0
|
||||
element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m);
|
||||
else
|
||||
element.c.(field{i}) = 0;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
%%
|
||||
function [stewart] = initializeParameters(stewart)
|
||||
%% Connection points on base and top plate w.r.t. World frame at the center of the base plate
|
||||
stewart.pos_base = zeros(6, 3);
|
||||
stewart.pos_top = zeros(6, 3);
|
||||
|
||||
alpha_b = stewart.BP.leg.ang*pi/180; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
||||
alpha_t = stewart.TP.leg.ang*pi/180; % +- offset angle from 120 degree spacing on top
|
||||
|
||||
height = (stewart.h-stewart.BP.thickness-stewart.TP.thickness-stewart.Leg.sphere.bottom-stewart.Leg.sphere.top-stewart.SP.thickness.bottom-stewart.SP.thickness.top)*0.001; % TODO
|
||||
|
||||
radius_b = stewart.BP.leg.rad*0.001; % rayon emplacement support base
|
||||
radius_t = stewart.TP.leg.rad*0.001; % top radius in meters
|
||||
|
||||
for i = 1:3
|
||||
% base points
|
||||
angle_m_b = (2*pi/3)* (i-1) - alpha_b;
|
||||
angle_p_b = (2*pi/3)* (i-1) + alpha_b;
|
||||
stewart.pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0];
|
||||
stewart.pos_base(2*i,:) = [radius_b*cos(angle_p_b), radius_b*sin(angle_p_b), 0.0];
|
||||
|
||||
% top points
|
||||
% Top points are 60 degrees offset
|
||||
angle_m_t = (2*pi/3)* (i-1) - alpha_t + 2*pi/6;
|
||||
angle_p_t = (2*pi/3)* (i-1) + alpha_t + 2*pi/6;
|
||||
stewart.pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height];
|
||||
stewart.pos_top(2*i,:) = [radius_t*cos(angle_p_t), radius_t*sin(angle_p_t), height];
|
||||
end
|
||||
|
||||
% permute pos_top points so that legs are end points of base and top points
|
||||
stewart.pos_top = [stewart.pos_top(6,:); stewart.pos_top(1:5,:)]; %6th point on top connects to 1st on bottom
|
||||
stewart.pos_top_tranform = stewart.pos_top - height*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% leg vectors
|
||||
legs = stewart.pos_top - stewart.pos_base;
|
||||
leg_length = zeros(6, 1);
|
||||
leg_vectors = zeros(6, 3);
|
||||
for i = 1:6
|
||||
leg_length(i) = norm(legs(i,:));
|
||||
leg_vectors(i,:) = legs(i,:) / leg_length(i);
|
||||
end
|
||||
|
||||
stewart.Leg.lenght = 1000*leg_length(1)/1.5;
|
||||
stewart.Leg.shape.bot = [0 0; ...
|
||||
stewart.Leg.rad.bottom 0; ...
|
||||
stewart.Leg.rad.bottom stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top stewart.Leg.lenght; ...
|
||||
stewart.Leg.rad.top 0.2*stewart.Leg.lenght; ...
|
||||
0 0.2*stewart.Leg.lenght];
|
||||
|
||||
%% Calculate revolute and cylindrical axes
|
||||
rev1 = zeros(6, 3);
|
||||
rev2 = zeros(6, 3);
|
||||
cyl1 = zeros(6, 3);
|
||||
for i = 1:6
|
||||
rev1(i,:) = cross(leg_vectors(i,:), [0 0 1]);
|
||||
rev1(i,:) = rev1(i,:) / norm(rev1(i,:));
|
||||
|
||||
rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:));
|
||||
rev2(i,:) = rev2(i,:) / norm(rev2(i,:));
|
||||
|
||||
cyl1(i,:) = leg_vectors(i,:);
|
||||
end
|
||||
|
||||
|
||||
%% Coordinate systems
|
||||
stewart.lower_leg = struct('rotation', eye(3));
|
||||
stewart.upper_leg = struct('rotation', eye(3));
|
||||
|
||||
for i = 1:6
|
||||
stewart.lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
stewart.upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
end
|
||||
|
||||
%% Position Matrix
|
||||
stewart.M_pos_base = stewart.pos_base + (height+(stewart.TP.thickness+stewart.Leg.sphere.top+stewart.SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% Compute Jacobian Matrix
|
||||
aa = stewart.pos_top_tranform + (stewart.jacobian - stewart.TP.thickness - stewart.SP.height.top)*1e-3*[zeros(6, 2),ones(6, 1)];
|
||||
stewart.J = getJacobianMatrix(leg_vectors', aa');
|
||||
end
|
||||
|
||||
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
|
||||
end
|
||||
%% Save
|
||||
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Cedrat Actuator
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
simscape_subsystems/nano_hexapod_leg_rigid.slx
Normal file
BIN
simscape_subsystems/nano_hexapod_leg_rigid.slx
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user