diff --git a/mat/controllers.mat b/mat/controllers.mat
index ffcfa92..7566f55 100644
Binary files a/mat/controllers.mat and b/mat/controllers.mat differ
diff --git a/mat/nass_disturbances.mat b/mat/nass_disturbances.mat
index 28de333..b3300ef 100644
Binary files a/mat/nass_disturbances.mat and b/mat/nass_disturbances.mat differ
diff --git a/mat/nass_references.mat b/mat/nass_references.mat
index cb9a5f8..72aeb03 100644
Binary files a/mat/nass_references.mat and b/mat/nass_references.mat differ
diff --git a/mat/stages.mat b/mat/stages.mat
index 168163e..87ca2a4 100644
Binary files a/mat/stages.mat and b/mat/stages.mat differ
diff --git a/simscape_subsystems/index.org b/simscape_subsystems/index.org
index 58ba6c6..6caef29 100644
--- a/simscape_subsystems/index.org
+++ b/simscape_subsystems/index.org
@@ -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
diff --git a/simscape_subsystems/nano_hexapod_D.slx b/simscape_subsystems/nano_hexapod_D.slx
index 1162c02..f29df82 100644
Binary files a/simscape_subsystems/nano_hexapod_D.slx and b/simscape_subsystems/nano_hexapod_D.slx differ
diff --git a/simscape_subsystems/nano_hexapod_F.slx b/simscape_subsystems/nano_hexapod_F.slx
index 6332b66..5ba00b0 100644
Binary files a/simscape_subsystems/nano_hexapod_F.slx and b/simscape_subsystems/nano_hexapod_F.slx differ
diff --git a/simscape_subsystems/nano_hexapod_F_sensors.slx b/simscape_subsystems/nano_hexapod_F_sensors.slx
deleted file mode 100644
index 3942914..0000000
Binary files a/simscape_subsystems/nano_hexapod_F_sensors.slx and /dev/null differ
diff --git a/simscape_subsystems/nano_hexapod_leg.slx b/simscape_subsystems/nano_hexapod_leg.slx
index ff77906..b0228c3 100644
Binary files a/simscape_subsystems/nano_hexapod_leg.slx and b/simscape_subsystems/nano_hexapod_leg.slx differ
diff --git a/simscape_subsystems/nano_hexapod_leg_rigid.slx b/simscape_subsystems/nano_hexapod_leg_rigid.slx
new file mode 100644
index 0000000..59b5341
Binary files /dev/null and b/simscape_subsystems/nano_hexapod_leg_rigid.slx differ
diff --git a/simscape_subsystems/nano_hexapod_rigid.slx b/simscape_subsystems/nano_hexapod_rigid.slx
index 2bdae8c..33a6251 100644
Binary files a/simscape_subsystems/nano_hexapod_rigid.slx and b/simscape_subsystems/nano_hexapod_rigid.slx differ
diff --git a/simscape_subsystems/nano_hexapod_rigid_simple.slx b/simscape_subsystems/nano_hexapod_rigid_simple.slx
index b0bbb4e..3233c31 100644
Binary files a/simscape_subsystems/nano_hexapod_rigid_simple.slx and b/simscape_subsystems/nano_hexapod_rigid_simple.slx differ
diff --git a/simscape_subsystems/granite_1dof.slx b/simscape_subsystems_uniaxial/granite_1dof.slx
similarity index 100%
rename from simscape_subsystems/granite_1dof.slx
rename to simscape_subsystems_uniaxial/granite_1dof.slx
diff --git a/simscape_subsystems/ground_1dof.slx b/simscape_subsystems_uniaxial/ground_1dof.slx
similarity index 100%
rename from simscape_subsystems/ground_1dof.slx
rename to simscape_subsystems_uniaxial/ground_1dof.slx
diff --git a/simscape_subsystems/micro_hexapod_1dof.slx b/simscape_subsystems_uniaxial/micro_hexapod_1dof.slx
similarity index 100%
rename from simscape_subsystems/micro_hexapod_1dof.slx
rename to simscape_subsystems_uniaxial/micro_hexapod_1dof.slx
diff --git a/simscape_subsystems/nano_hexapod_1dof.slx b/simscape_subsystems_uniaxial/nano_hexapod_1dof.slx
similarity index 100%
rename from simscape_subsystems/nano_hexapod_1dof.slx
rename to simscape_subsystems_uniaxial/nano_hexapod_1dof.slx
diff --git a/simscape_subsystems/sample_environment_1dof.slx b/simscape_subsystems_uniaxial/sample_environment_1dof.slx
similarity index 100%
rename from simscape_subsystems/sample_environment_1dof.slx
rename to simscape_subsystems_uniaxial/sample_environment_1dof.slx
diff --git a/simscape_subsystems/sample_environment_1dof_rigid.slx b/simscape_subsystems_uniaxial/sample_environment_1dof_rigid.slx
similarity index 100%
rename from simscape_subsystems/sample_environment_1dof_rigid.slx
rename to simscape_subsystems_uniaxial/sample_environment_1dof_rigid.slx
diff --git a/simscape_subsystems/spindle_1dof.slx b/simscape_subsystems_uniaxial/spindle_1dof.slx
similarity index 100%
rename from simscape_subsystems/spindle_1dof.slx
rename to simscape_subsystems_uniaxial/spindle_1dof.slx
diff --git a/simscape_subsystems/tilt_stage_1dof.slx b/simscape_subsystems_uniaxial/tilt_stage_1dof.slx
similarity index 100%
rename from simscape_subsystems/tilt_stage_1dof.slx
rename to simscape_subsystems_uniaxial/tilt_stage_1dof.slx
diff --git a/simscape_subsystems/translation_stage_1dof.slx b/simscape_subsystems_uniaxial/translation_stage_1dof.slx
similarity index 100%
rename from simscape_subsystems/translation_stage_1dof.slx
rename to simscape_subsystems_uniaxial/translation_stage_1dof.slx
diff --git a/src/computeJacobian.m b/src/computeJacobian.m
new file mode 100644
index 0000000..80200a0
--- /dev/null
+++ b/src/computeJacobian.m
@@ -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);
diff --git a/src/computeJointsPose.m b/src/computeJointsPose.m
new file mode 100644
index 0000000..5b5754e
--- /dev/null
+++ b/src/computeJointsPose.m
@@ -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
diff --git a/src/forwardKinematicsApprox.m b/src/forwardKinematicsApprox.m
new file mode 100644
index 0000000..942b92a
--- /dev/null
+++ b/src/forwardKinematicsApprox.m
@@ -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)];
diff --git a/src/generateCubicConfiguration.m b/src/generateCubicConfiguration.m
new file mode 100644
index 0000000..8bc48dc
--- /dev/null
+++ b/src/generateCubicConfiguration.m
@@ -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;
diff --git a/src/generateGeneralConfiguration.m b/src/generateGeneralConfiguration.m
new file mode 100644
index 0000000..4fd7064
--- /dev/null
+++ b/src/generateGeneralConfiguration.m
@@ -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
diff --git a/src/initializeCylindricalPlatforms.m b/src/initializeCylindricalPlatforms.m
new file mode 100644
index 0000000..6f93b1f
--- /dev/null
+++ b/src/initializeCylindricalPlatforms.m
@@ -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;
diff --git a/src/initializeCylindricalStruts.m b/src/initializeCylindricalStruts.m
new file mode 100644
index 0000000..a83d0c5
--- /dev/null
+++ b/src/initializeCylindricalStruts.m
@@ -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;
diff --git a/src/initializeFramesPositions.m b/src/initializeFramesPositions.m
new file mode 100644
index 0000000..efb99a5
--- /dev/null
+++ b/src/initializeFramesPositions.m
@@ -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]
diff --git a/src/initializeNanoHexapod.m b/src/initializeNanoHexapod.m
index ea7af28..cc184a7 100644
--- a/src/initializeNanoHexapod.m
+++ b/src/initializeNanoHexapod.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
diff --git a/src/initializeStrutDynamics.m b/src/initializeStrutDynamics.m
new file mode 100644
index 0000000..e4ba57f
--- /dev/null
+++ b/src/initializeStrutDynamics.m
@@ -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;
diff --git a/src/inverseKinematics.m b/src/inverseKinematics.m
new file mode 100644
index 0000000..a214e1f
--- /dev/null
+++ b/src/inverseKinematics.m
@@ -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;
diff --git a/stewart_platform/index.org b/stewart_platform/index.org
new file mode 100644
index 0000000..c649460
--- /dev/null
+++ b/stewart_platform/index.org
@@ -0,0 +1,712 @@
+#+TITLE: Stewart Platform - Simscape Model
+:DRAWER:
+#+HTML_LINK_HOME: ./index.html
+#+HTML_LINK_UP: ./index.html
+
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+#+HTML_HEAD:
+
+#+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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:
+<>
+
+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