diff --git a/stewart-simscape/getJacobianMatrix.m b/stewart-simscape/getJacobianMatrix.m deleted file mode 100644 index c437493..0000000 --- a/stewart-simscape/getJacobianMatrix.m +++ /dev/null @@ -1,18 +0,0 @@ -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 diff --git a/stewart-simscape/stewart_init.m b/stewart-simscape/stewart_init.m deleted file mode 100644 index 3e1a710..0000000 --- a/stewart-simscape/stewart_init.m +++ /dev/null @@ -1,89 +0,0 @@ -%% Define some constant values -deg2rad = pi/180; -x_axis = [1 0 0]; -y_axis = [0 1 0]; -z_axis = [0 0 1]; - -%% Connection points on base and top plate w.r.t. World frame at the center of the base plate -pos_base = zeros(6, 3); -pos_top = zeros(6, 3); - -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 = (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 - % base points - angle_m_b = (2*pi/3)* (i-1) - alpha_b; - angle_p_b = (2*pi/3)* (i-1) + alpha_b; - pos_base(2*i-1,:) = [radius_b*cos(angle_m_b), radius_b*sin(angle_m_b), 0.0]; - 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; - pos_top(2*i-1,:) = [radius_t*cos(angle_m_t), radius_t*sin(angle_m_t), height]; - 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 -pos_top = [pos_top(6,:); pos_top(1:5,:)]; %6th point on top connects to 1st on bottom -pos_top_tranform = pos_top - height*[zeros(6, 2),ones(6, 1)]; - -%% Compute points w.r.t. to the body frame in a 3x6 matrix -body_pts = pos_top' - height*[zeros(2,6);ones(1,6)]; - -%% leg vectors -legs = pos_top - 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 - -Leg.lenght = 1000*leg_length(1)/1.5; - -%% Calculate revolute and cylindrical axes -rev1 = zeros(6, 3); -rev2 = zeros(6, 3); -rev3 = zeros(6, 3); -rev4 = zeros(6, 3); -cyl1 = zeros(6, 3); -for i = 1:6 - rev1(i,:) = cross(leg_vectors(i,:), z_axis); - rev1(i,:) = rev1(i,:) / norm(rev1(i,:)); - rev3(i,:) = rev1(i,:); - - rev2(i,:) = - cross(rev1(i,:), leg_vectors(i,:)); - rev2(i,:) = rev2(i,:) / norm(rev2(i,:)); - rev4(i,:) = rev2(i,:); - - cyl1(i,:) = leg_vectors(i,:); -end - - -%% Coordinate systems -lower_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]); -upper_leg = struct('origin', [0 0 0], 'rotation', eye(3), 'end_point', [0 0 0]); - -for i = 1:6 - lower_leg(i).origin = pos_base(i,:) + (3/8)*legs(i,:); - lower_leg(i).end_point = pos_base(i,:) + (3/4)*legs(i,:); - lower_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; - upper_leg(i).origin = pos_base(i,:) + (1-3/8)*legs(i,:); - upper_leg(i).end_point = pos_base(i,:) + (1/4)*legs(i,:); - upper_leg(i).rotation = [rev1(i,:)', rev2(i,:)', cyl1(i,:)']; -end - -%% 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); - diff --git a/stewart-simscape/stewart_parameters.m b/stewart-simscape/stewart_parameters.m deleted file mode 100644 index 4b6e086..0000000 --- a/stewart-simscape/stewart_parameters.m +++ /dev/null @@ -1,66 +0,0 @@ -%% Nass height -stewart = struct(); -stewart.h = 90; %mm -stewart.jacobian = 174.5; %mm - -%% Bottom Plate -BP = struct(); -BP.rad.int = 0; %mm -BP.rad.ext = 150; %mm -BP.thickness = 10; % mm -BP.leg.rad = 100; %mm -BP.leg.ang = 5; %deg -BP.density = 8000; %kg/m^3 -BP.color = [0.5 0.5 0.5]; %rgb - -%% TOP Plate -TP = struct(); -TP.rad.int = 0;%mm -TP.rad.ext = 100; %mm -TP.thickness = 10; % mm -TP.leg.rad = 90; %mm -TP.leg.ang = 5; %deg -TP.density = 8000; %kg/m^3 -TP.color = [0.5 0.5 0.5]; %rgb - -%% Leg -Leg = struct(); -Leg.stroke = 80e-6; % m -Leg.rad.bottom = 8; %mm -Leg.rad.top = 5; %mm -Leg.sphere.bottom = 10; % mm -Leg.sphere.top = 8; % mm -Leg.density = 8000; %kg/m^3 -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 -Leg.k.ax = 5e7; % N/m -Leg.ksi.ax = 10; -Leg = updateDamping(Leg); - - -%% Sphere -SP = struct(); -SP.thickness.bottom = 1; %mm -SP.thickness.top = 1; %mm -SP.rad.bottom = Leg.sphere.bottom; %mm -SP.rad.top = Leg.sphere.top; %mm -SP.height.bottom = 5; %mm -SP.height.top = 5; %mm -SP.density.bottom = 8000; %kg/m^3 -SP.density.top = 8000; %kg/m^3 -SP.m = SP.density.bottom*2*pi*((SP.rad.bottom*1e-3)^2)*(SP.height.bottom*1e-3); %kg -SP.color.bottom = [0.5 0.5 0.5]; %rgb -SP.color.top = [0.5 0.5 0.5]; %rgb -SP.k.ax = 0; % N*m/deg -SP.ksi.ax = 10; -SP = updateDamping(SP); - -%% -function element = updateDamping(element) - field = fieldnames(element.k); - for i = 1:length(field) - element.c.(field{i}) = 1/element.ksi.(field{i})*sqrt(element.k.(field{i})/element.m); - end -end