[BRK] Update the nano-hexapod Simscape model
This commit is contained in:
parent
6bce9108cd
commit
24f44f5477
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
mat/stages.mat
BIN
mat/stages.mat
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.
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.
21
src/computeJacobian.m
Normal file
21
src/computeJacobian.m
Normal file
@ -0,0 +1,21 @@
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - K [6x6] - The Stiffness Matrix
|
||||
% - C [6x6] - The Compliance Matrix
|
||||
|
||||
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
|
||||
|
||||
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
|
||||
|
||||
stewart.C = inv(stewart.K);
|
||||
47
src/computeJointsPose.m
Normal file
47
src/computeJointsPose.m
Normal file
@ -0,0 +1,47 @@
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - l [6x1] - The i'th element is the initial length of strut i
|
||||
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
|
||||
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
|
||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
||||
|
||||
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
|
||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
||||
|
||||
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
|
||||
|
||||
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
|
||||
|
||||
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
|
||||
|
||||
stewart.FRa = zeros(3,3,6);
|
||||
stewart.MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
|
||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
|
||||
|
||||
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
|
||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
||||
end
|
||||
31
src/forwardKinematicsApprox.m
Normal file
31
src/forwardKinematicsApprox.m
Normal file
@ -0,0 +1,31 @@
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
|
||||
X = stewart.J\args.dL;
|
||||
|
||||
P = X(1:3);
|
||||
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
44
src/generateCubicConfiguration.m
Normal file
44
src/generateCubicConfiguration.m
Normal file
@ -0,0 +1,44 @@
|
||||
function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
% generateCubicConfiguration - Generate a Cubic Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||||
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
end
|
||||
|
||||
sx = [ 2; -1; -1];
|
||||
sy = [ 0; 1; -1];
|
||||
sz = [ 1; 1; 1];
|
||||
|
||||
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
||||
|
||||
L = args.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*args.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
|
||||
|
||||
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||
|
||||
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
36
src/generateGeneralConfiguration.m
Normal file
36
src/generateGeneralConfiguration.m
Normal file
@ -0,0 +1,36 @@
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-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} = 70e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
|
||||
stewart.Fa = zeros(3,6);
|
||||
stewart.Mb = zeros(3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
53
src/initializeCylindricalPlatforms.m
Normal file
53
src/initializeCylindricalPlatforms.m
Normal file
@ -0,0 +1,53 @@
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platforms [struct] - structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-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
|
||||
end
|
||||
|
||||
platforms = struct();
|
||||
|
||||
platforms.Fpm = args.Fpm;
|
||||
platforms.Fph = args.Fph;
|
||||
platforms.Fpr = args.Fpr;
|
||||
platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/2 * platforms.Fpm * platforms.Fpr^2]);
|
||||
|
||||
platforms.Mpm = args.Mpm;
|
||||
platforms.Mph = args.Mph;
|
||||
platforms.Mpr = args.Mpr;
|
||||
platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/2 * platforms.Mpm * platforms.Mpr^2]);
|
||||
|
||||
stewart.platforms = platforms;
|
||||
58
src/initializeCylindricalStruts.m
Normal file
58
src/initializeCylindricalStruts.m
Normal file
@ -0,0 +1,58 @@
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts [struct] - structure with the following fields:
|
||||
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - Msm [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
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
|
||||
end
|
||||
|
||||
struts = struct();
|
||||
|
||||
struts.Fsm = ones(6,1).*args.Fsm;
|
||||
struts.Msm = ones(6,1).*args.Msm;
|
||||
|
||||
struts.Fsh = ones(6,1).*args.Fsh;
|
||||
struts.Fsr = ones(6,1).*args.Fsr;
|
||||
struts.Msh = ones(6,1).*args.Msh;
|
||||
struts.Msr = ones(6,1).*args.Msr;
|
||||
|
||||
struts.Fsi = zeros(3, 3, 6);
|
||||
struts.Msi = zeros(3, 3, 6);
|
||||
for i = 1:6
|
||||
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
|
||||
struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
|
||||
end
|
||||
|
||||
stewart.struts = struts;
|
||||
31
src/initializeFramesPositions.m
Normal file
31
src/initializeFramesPositions.m
Normal file
@ -0,0 +1,31 @@
|
||||
function [stewart] = initializeFramesPositions(args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
|
||||
arguments
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
|
||||
stewart = struct();
|
||||
|
||||
stewart.H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
||||
@ -1,202 +1,62 @@
|
||||
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)
|
||||
% 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();
|
||||
|
||||
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];
|
||||
|
||||
%% Leg
|
||||
Leg = struct();
|
||||
|
||||
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]
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e7*ones(6,1), ...
|
||||
'Ci', 1e2*ones(6,1));
|
||||
elseif strcmp(args.actuator, 'lorentz')
|
||||
Leg.k.ax = 1e4; % Stiffness of each leg [N/m]
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e4*ones(6,1), ...
|
||||
'Ci', 1e2*ones(6,1));
|
||||
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]
|
||||
|
||||
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]
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpm', args.Fpm, 'Fph', args.Fph, 'Fpr', args.Fpr, 'Mpm', args.Mpm, 'Mph', args.Mph, 'Mpr', args.Mpr);
|
||||
|
||||
Leg = updateDamping(Leg);
|
||||
stewart = initializeCylindricalStruts(stewart, 'Fsm', args.Fsm, 'Fsh', args.Fsh, 'Fsr', args.Fsr, 'Msm', args.Msm, 'Msh', args.Msh, 'Msr', args.Msr);
|
||||
|
||||
stewart = computeJacobian(stewart);
|
||||
|
||||
%% Sphere
|
||||
SP = struct();
|
||||
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
||||
|
||||
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);
|
||||
nano_hexapod = stewart;
|
||||
|
||||
%% 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
|
||||
|
||||
23
src/initializeStrutDynamics.m
Normal file
23
src/initializeStrutDynamics.m
Normal file
@ -0,0 +1,23 @@
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||
end
|
||||
|
||||
stewart.Ki = args.Ki;
|
||||
stewart.Ci = args.Ci;
|
||||
26
src/inverseKinematics.m
Normal file
26
src/inverseKinematics.m
Normal file
@ -0,0 +1,26 @@
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
|
||||
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
|
||||
|
||||
dLi = Li-stewart.l;
|
||||
712
stewart_platform/index.org
Normal file
712
stewart_platform/index.org
Normal file
@ -0,0 +1,712 @@
|
||||
#+TITLE: Stewart Platform - Simscape Model
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
Stewart platforms are generated in multiple steps.
|
||||
|
||||
We define 4 important *frames*:
|
||||
- $\{F\}$: Frame fixed to the *Fixed* base and located at the center of its bottom surface.
|
||||
This is used to fix the Stewart platform to some support.
|
||||
- $\{M\}$: Frame fixed to the *Moving* platform and located at the center of its top surface.
|
||||
This is used to place things on top of the Stewart platform.
|
||||
- $\{A\}$: Frame fixed to the fixed base.
|
||||
It defined the center of rotation of the moving platform.
|
||||
- $\{B\}$: Frame fixed to the moving platform.
|
||||
The motion of the moving platforms and forces applied to it are defined with respect to this frame $\{B\}$.
|
||||
|
||||
Then, we define the *location of the spherical joints*:
|
||||
- $\bm{a}_{i}$ are the position of the spherical joints fixed to the fixed base
|
||||
- $\bm{b}_{i}$ are the position of the spherical joints fixed to the moving platform
|
||||
|
||||
We define the *rest position* of the Stewart platform:
|
||||
- For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.
|
||||
- Thus, to define the rest position of the Stewart platform, we just have to defined its total height $H$.
|
||||
$H$ corresponds to the distance from the bottom of the fixed base to the top of the moving platform.
|
||||
|
||||
From $\bm{a}_{i}$ and $\bm{b}_{i}$, we can determine the *length and orientation of each strut*:
|
||||
- $l_{i}$ is the length of the strut
|
||||
- ${}^{A}\hat{\bm{s}}_{i}$ is the unit vector align with the strut
|
||||
|
||||
The position of the Spherical joints can be computed using various methods:
|
||||
- Cubic configuration
|
||||
- Circular configuration
|
||||
- Arbitrary position
|
||||
- These methods should be easily scriptable and corresponds to specific functions that returns ${}^{F}\bm{a}_{i}$ and ${}^{M}\bm{b}_{i}$.
|
||||
The input of these functions are the parameters corresponding to the wanted geometry.
|
||||
|
||||
For Simscape, we need:
|
||||
- The position and orientation of each spherical joint fixed to the fixed base: ${}^{F}\bm{a}_{i}$ and ${}^{F}\bm{R}_{a_{i}}$
|
||||
- The position and orientation of each spherical joint fixed to the moving platform: ${}^{M}\bm{b}_{i}$ and ${}^{M}\bm{R}_{b_{i}}$
|
||||
- The rest length of each strut: $l_{i}$
|
||||
- The stiffness and damping of each actuator: $k_{i}$ and $c_{i}$
|
||||
- The position of the frame $\{A\}$ with respect to the frame $\{F\}$: ${}^{F}\bm{O}_{A}$
|
||||
- The position of the frame $\{B\}$ with respect to the frame $\{M\}$: ${}^{M}\bm{O}_{B}$
|
||||
|
||||
* =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeFramesPositions.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeFramesPositions>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeFramesPositions.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeFramesPositions(args)
|
||||
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
||||
%
|
||||
% Syntax: [stewart] = initializeFramesPositions(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
||||
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following fields:
|
||||
% - H [1x1] - Total Height of the Stewart Platform [m]
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F} [m]
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M} [m]
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F} [m]
|
||||
#+end_src
|
||||
|
||||
** Documentation
|
||||
|
||||
#+name: fig:stewart-frames-position
|
||||
#+caption: Definition of the position of the frames
|
||||
[[file:figs/stewart-frames-position.png]]
|
||||
|
||||
** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
||||
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart structure
|
||||
#+begin_src matlab
|
||||
stewart = struct();
|
||||
#+end_src
|
||||
|
||||
** Compute the position of each frame
|
||||
#+begin_src matlab
|
||||
stewart.H = args.H; % Total Height of the Stewart Platform [m]
|
||||
|
||||
stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m]
|
||||
|
||||
stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
||||
|
||||
stewart.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m]
|
||||
#+end_src
|
||||
|
||||
* Initialize the position of the Joints
|
||||
** =generateCubicConfiguration=: Generate a Cubic Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/generateCubicConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:generateCubicConfiguration>>
|
||||
|
||||
This Matlab function is accessible [[file:src/generateCubicConfiguration.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
% generateCubicConfiguration - Generate a Cubic Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||||
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
#+end_src
|
||||
|
||||
*** Documentation
|
||||
#+name: fig:cubic-configuration-definition
|
||||
#+caption: Cubic Configuration
|
||||
[[file:figs/cubic-configuration-definition.png]]
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Position of the Cube
|
||||
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 = args.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*args.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
|
||||
|
||||
*** Compute the pose
|
||||
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
|
||||
stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
#+end_src
|
||||
|
||||
** =generateGeneralConfiguration=: Generate a Very General Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/generateGeneralConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:generateGeneralConfiguration>>
|
||||
|
||||
This Matlab function is accessible [[file:src/generateGeneralConfiguration.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
% generateGeneralConfiguration - Generate a Very General Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Can have the following fields:
|
||||
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
||||
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
||||
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
||||
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
||||
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
||||
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
#+end_src
|
||||
|
||||
*** Documentation
|
||||
Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}.
|
||||
The radius of the circles can be chosen as well as the angles where the joints are located.
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
||||
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-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} = 70e-3;
|
||||
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Compute the pose
|
||||
#+begin_src matlab
|
||||
stewart.Fa = zeros(3,6);
|
||||
stewart.Mb = zeros(3,6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
for i = 1:6
|
||||
stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
||||
stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* =computeJointsPose=: Compute the Pose of the Joints
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeJointsPose.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeJointsPose>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeJointsPose.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJointsPose(stewart)
|
||||
% computeJointsPose -
|
||||
%
|
||||
% Syntax: [stewart] = computeJointsPose(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
% - FO_A [3x1] - Position of {A} with respect to {F}
|
||||
% - MO_B [3x1] - Position of {B} with respect to {M}
|
||||
% - FO_M [3x1] - Position of {M} with respect to {F}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - A structure with the following added fields
|
||||
% - Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
||||
% - Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
||||
% - Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
||||
% - Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
||||
% - l [6x1] - The i'th element is the initial length of strut i
|
||||
% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
||||
% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
||||
% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
||||
% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
||||
#+end_src
|
||||
|
||||
** Documentation
|
||||
|
||||
#+name: fig:stewart-struts
|
||||
#+caption: Position and orientation of the struts
|
||||
[[file:figs/stewart-struts.png]]
|
||||
|
||||
** Compute the position of the Joints
|
||||
#+begin_src matlab
|
||||
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
|
||||
stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
|
||||
|
||||
stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]);
|
||||
stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]);
|
||||
#+end_src
|
||||
|
||||
** Compute the strut length and orientation
|
||||
#+begin_src matlab
|
||||
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
|
||||
|
||||
stewart.l = vecnorm(stewart.Ab - stewart.Aa)';
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba);
|
||||
#+end_src
|
||||
|
||||
** Compute the orientation of the Joints
|
||||
#+begin_src matlab
|
||||
stewart.FRa = zeros(3,3,6);
|
||||
stewart.MRb = zeros(3,3,6);
|
||||
|
||||
for i = 1:6
|
||||
stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)];
|
||||
stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i));
|
||||
|
||||
stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)];
|
||||
stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i));
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeStrutDynamics.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeStrutDynamics>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeStrutDynamics(stewart, args)
|
||||
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
||||
%
|
||||
% Syntax: [stewart] = initializeStrutDynamics(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - Ki [6x1] - Stiffness of each strut [N/m]
|
||||
% - Ci [6x1] - Damping of each strut [N/(m/s)]
|
||||
#+end_src
|
||||
|
||||
** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
|
||||
args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Add Stiffness and Damping properties of each strut
|
||||
#+begin_src matlab
|
||||
stewart.Ki = args.Ki;
|
||||
stewart.Ci = args.Ci;
|
||||
#+end_src
|
||||
|
||||
* =computeJacobian=: Compute the Jacobian Matrix
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/computeJacobian.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeJacobian>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
|
||||
** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - K [6x6] - The Stiffness Matrix
|
||||
% - C [6x6] - The Compliance Matrix
|
||||
#+end_src
|
||||
|
||||
** Compute Jacobian Matrix
|
||||
#+begin_src matlab
|
||||
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
|
||||
#+end_src
|
||||
|
||||
** Compute Stiffness Matrix
|
||||
#+begin_src matlab
|
||||
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
|
||||
#+end_src
|
||||
|
||||
** Compute Compliance Matrix
|
||||
#+begin_src matlab
|
||||
stewart.C = inv(stewart.K);
|
||||
#+end_src
|
||||
|
||||
* Initialize the Geometry of the Mechanical Elements
|
||||
** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeCylindricalPlatforms.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeCylindricalPlatforms>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeCylindricalPlatforms.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
||||
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platforms [struct] - structure with the following fields:
|
||||
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
||||
% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
||||
% - Fph [1x1] - Fixed Platform Height [m]
|
||||
% - Fpr [1x1] - Fixed Platform Radius [m]
|
||||
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
||||
% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
||||
% - Mph [1x1] - Mobile Platform Height [m]
|
||||
% - Mpr [1x1] - Mobile Platform Radius [m]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
||||
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
||||
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-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
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Create the =platforms= struct
|
||||
#+begin_src matlab
|
||||
platforms = struct();
|
||||
|
||||
platforms.Fpm = args.Fpm;
|
||||
platforms.Fph = args.Fph;
|
||||
platforms.Fpr = args.Fpr;
|
||||
platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ...
|
||||
1/2 * platforms.Fpm * platforms.Fpr^2]);
|
||||
|
||||
platforms.Mpm = args.Mpm;
|
||||
platforms.Mph = args.Mph;
|
||||
platforms.Mpr = args.Mpr;
|
||||
platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ...
|
||||
1/2 * platforms.Mpm * platforms.Mpr^2]);
|
||||
#+end_src
|
||||
|
||||
*** Save the =platforms= struct
|
||||
#+begin_src matlab
|
||||
stewart.platforms = platforms;
|
||||
#+end_src
|
||||
|
||||
** =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/initializeCylindricalStruts.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeCylindricalStruts>>
|
||||
|
||||
This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [stewart] = initializeCylindricalStruts(stewart, args)
|
||||
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
||||
%
|
||||
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
||||
%
|
||||
% Inputs:
|
||||
% - args - Structure with the following fields:
|
||||
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - struts [struct] - structure with the following fields:
|
||||
% - Fsm [6x1] - Mass of the Fixed part of the struts [kg]
|
||||
% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
||||
% - Msm [6x1] - Mass of the Mobile part of the struts [kg]
|
||||
% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
||||
% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
||||
% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
||||
% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
||||
% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
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
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Create the =struts= structure
|
||||
#+begin_src matlab
|
||||
struts = struct();
|
||||
|
||||
struts.Fsm = ones(6,1).*args.Fsm;
|
||||
struts.Msm = ones(6,1).*args.Msm;
|
||||
|
||||
struts.Fsh = ones(6,1).*args.Fsh;
|
||||
struts.Fsr = ones(6,1).*args.Fsr;
|
||||
struts.Msh = ones(6,1).*args.Msh;
|
||||
struts.Msr = ones(6,1).*args.Msr;
|
||||
|
||||
struts.Fsi = zeros(3, 3, 6);
|
||||
struts.Msi = zeros(3, 3, 6);
|
||||
for i = 1:6
|
||||
struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ...
|
||||
1/2 * struts.Fsm(i) * struts.Fsr(i)^2]);
|
||||
struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ...
|
||||
1/2 * struts.Msm(i) * struts.Msr(i)^2]);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
stewart.struts = struts;
|
||||
#+end_src
|
||||
|
||||
* Utility Functions
|
||||
** =inverseKinematics=: Compute Inverse Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/inverseKinematics.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:inverseKinematics>>
|
||||
|
||||
This Matlab function is accessible [[file:src/inverseKinematics.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - Aa [3x6] - The positions ai expressed in {A}
|
||||
% - Bb [3x6] - The positions bi expressed in {B}
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Theory
|
||||
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
||||
|
||||
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
||||
\begin{align*}
|
||||
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
||||
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
||||
\end{align*}
|
||||
|
||||
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
||||
\begin{equation}
|
||||
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
||||
\end{equation}
|
||||
|
||||
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||||
\begin{equation}
|
||||
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
||||
\end{equation}
|
||||
|
||||
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
||||
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
||||
|
||||
*** Compute
|
||||
#+begin_src matlab
|
||||
Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dLi = Li-stewart.l;
|
||||
#+end_src
|
||||
|
||||
** =forwardKinematicsApprox=: Compute the Forward Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ../src/forwardKinematicsApprox.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:forwardKinematicsApprox>>
|
||||
|
||||
This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].
|
||||
|
||||
*** Function description
|
||||
#+begin_src matlab
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Computation
|
||||
From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
#+begin_src matlab
|
||||
X = stewart.J\args.dL;
|
||||
#+end_src
|
||||
|
||||
The position vector corresponds to the first 3 elements.
|
||||
#+begin_src matlab
|
||||
P = X(1:3);
|
||||
#+end_src
|
||||
|
||||
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
||||
using the screw axis.
|
||||
#+begin_src matlab
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
#+end_src
|
||||
|
||||
We then compute the corresponding rotation matrix.
|
||||
#+begin_src matlab
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
#+end_src
|
||||
Loading…
Reference in New Issue
Block a user