Rename many files and add functions
This commit is contained in:
parent
5b516fa730
commit
6e4d033fd2
@ -1,27 +0,0 @@
|
||||
%% Position Matrix
|
||||
M_pos_base = pos_base + (height+(TP.thickness+Leg.sphere.top+SP.thickness.top+Nass.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
%% Rotation Matrix
|
||||
RM = leg_vectors;
|
||||
|
||||
%%
|
||||
J = computeJacobian(RM, M_pos_base);
|
||||
|
||||
%% Jacobian Matrix
|
||||
function J = computeJacobian(RM,M_pos_base)
|
||||
J = zeros(6);
|
||||
J(:, 1:3) = RM;
|
||||
for i = 1:6
|
||||
J(i, 4:6) = -RM(i, :)*getCrossProductMatrix(M_pos_base(i, :));
|
||||
end
|
||||
|
||||
function M = getCrossProductMatrix(v)
|
||||
M = zeros(3);
|
||||
M(1, 2) = -v(3);
|
||||
M(1, 3) = v(2);
|
||||
M(2, 3) = -v(1);
|
||||
M(2, 1) = -M(1, 2);
|
||||
M(3, 1) = -M(1, 3);
|
||||
M(3, 2) = -M(2, 3);
|
||||
end
|
||||
end
|
BIN
Nass_Matlab.slx
BIN
Nass_Matlab.slx
Binary file not shown.
@ -1,23 +0,0 @@
|
||||
lmax = 80e-6;
|
||||
|
||||
theta = linspace(0, 2*pi, 100);
|
||||
phi = linspace(-pi/2 , pi/2, 100);
|
||||
dmax = zeros(length(theta), length(phi));
|
||||
|
||||
for i = 1:length(theta)
|
||||
for j = 1:length(phi)
|
||||
L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
|
||||
lmaxbis = max(abs(L));
|
||||
dmax(i, j) = lmax/lmaxbis;
|
||||
end
|
||||
end
|
||||
|
||||
X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
|
||||
Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
|
||||
Z = dmax.*sin(repmat(phi,length(theta),1));
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
mesh(X, Y, Z);
|
||||
colorbar;
|
||||
hold off;
|
18
getJacobianMatrix.m
Normal file
18
getJacobianMatrix.m
Normal file
@ -0,0 +1,18 @@
|
||||
function J = getJacobianMatrix(RM,M_pos_base)
|
||||
J = zeros(6);
|
||||
|
||||
J(:, 1:3) = RM;
|
||||
for i = 1:6
|
||||
J(i, 4:6) = -RM(i, :)*getCrossProductMatrix(M_pos_base(i, :));
|
||||
end
|
||||
|
||||
function M = getCrossProductMatrix(v)
|
||||
M = zeros(3);
|
||||
M(1, 2) = -v(3);
|
||||
M(1, 3) = v(2);
|
||||
M(2, 3) = -v(1);
|
||||
M(2, 1) = -M(1, 2);
|
||||
M(3, 1) = -M(1, 3);
|
||||
M(3, 2) = -M(2, 3);
|
||||
end
|
||||
end
|
16
getMaxPositions.m
Normal file
16
getMaxPositions.m
Normal file
@ -0,0 +1,16 @@
|
||||
function [X, Y, Z] = getMaxPositions(lmax, J)
|
||||
theta = linspace(0, 2*pi, 100);
|
||||
phi = linspace(-pi/2 , pi/2, 100);
|
||||
dmax = zeros(length(theta), length(phi));
|
||||
|
||||
for i = 1:length(theta)
|
||||
for j = 1:length(phi)
|
||||
L = J*[cos(phi(j))*cos(theta(i)) cos(phi(j))*sin(theta(i)) sin(phi(j)) 0 0 0]';
|
||||
dmax(i, j) = lmax/max(abs(L));
|
||||
end
|
||||
end
|
||||
|
||||
X = dmax.*cos(repmat(phi,length(theta),1)).*cos(repmat(theta,length(phi),1))';
|
||||
Y = dmax.*cos(repmat(phi,length(theta),1)).*sin(repmat(theta,length(phi),1))';
|
||||
Z = dmax.*sin(repmat(phi,length(theta),1));
|
||||
end
|
10
plot_max_positions.m
Normal file
10
plot_max_positions.m
Normal file
@ -0,0 +1,10 @@
|
||||
lmax = 80e-6;
|
||||
|
||||
[X, Y, Z] = getMaxPositions(lmax, J);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
mesh(X, Y, Z);
|
||||
colorbar;
|
||||
hold off;
|
||||
|
@ -1,8 +1,10 @@
|
||||
%%
|
||||
clear;
|
||||
close all;
|
||||
clc;
|
||||
|
||||
%%
|
||||
run Design_Nass.m
|
||||
run stewart_parameters.m
|
||||
|
||||
%%
|
||||
deg2rad = pi/180;
|
||||
@ -16,7 +18,7 @@ pos_base = [];
|
||||
pos_top = [];
|
||||
alpha_b = BP.leg.ang*deg2rad; % angle de décalage par rapport à 120 deg (pour positionner les supports bases)
|
||||
alpha_t = TP.leg.ang*deg2rad; % +- offset angle from 120 degree spacing on top
|
||||
height = (Nass.h-BP.thickness-TP.thickness-Leg.sphere.bottom-Leg.sphere.top-SP.thickness.bottom-SP.thickness.top)*0.001 ; % 2 meter height in home configuration
|
||||
height = (stewart.h-BP.thickness-TP.thickness-Leg.sphere.bottom-Leg.sphere.top-SP.thickness.bottom-SP.thickness.top)*0.001 ; % 2 meter height in home configuration
|
||||
radius_b = BP.leg.rad*0.001; % rayon emplacement support base
|
||||
radius_t = TP.leg.rad*0.001; % top radius in meters
|
||||
for i = 1:3
|
||||
@ -75,5 +77,9 @@ for i = 1:6
|
||||
upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)'];
|
||||
end
|
||||
|
||||
%%
|
||||
run JacobianMatrix.m
|
||||
% Position Matrix
|
||||
M_pos_base = pos_base + (height+(TP.thickness+Leg.sphere.top+SP.thickness.top+stewart.jacobian)*1e-3)*[zeros(6, 2),ones(6, 1)];
|
||||
|
||||
% Compute Jacobian Matrix
|
||||
J = getJacobianMatrix(leg_vectors, M_pos_base);
|
||||
|
@ -1,7 +1,7 @@
|
||||
%% Nass height
|
||||
Nass = struct();
|
||||
Nass.h = 90; %mm
|
||||
Nass.jacobian = 174.5; %mm
|
||||
stewart = struct();
|
||||
stewart.h = 90; %mm
|
||||
stewart.jacobian = 174.5; %mm
|
||||
|
||||
%% Bottom Plate
|
||||
BP = struct();
|
||||
@ -30,7 +30,7 @@ Leg.rad.top = 5 ; %mm
|
||||
Leg.sphere.bottom = 10 ; % mm
|
||||
Leg.sphere.top = 8 ; % mm
|
||||
Leg.density = 8000 ; %kg/m^3
|
||||
Leg.lenght = Nass.h; % mm (approximate)
|
||||
Leg.lenght = stewart.h; % mm (approximate)
|
||||
Leg.m = Leg.density*2*pi*((Leg.rad.bottom*1e-3)^2)*(Leg.lenght*1e-3); %kg
|
||||
Leg.color.bottom = [0.5 0.5 0.5] ; %rgb
|
||||
Leg.color.top = [0.5 0.5 0.5] ; %rgb
|
BIN
stewart_simscape.slx
Normal file
BIN
stewart_simscape.slx
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user