Update Nano-Hexapod Model

.SimulinkProject/Root.type.Files/STEPS.type.File.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info />

.SimulinkProject/Root.type.Files/STEPS.type.File/1.type.DIR_SIGNIFIER.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info />

.SimulinkProject/Root.type.Files/STEPS.type.File/nano_hexapod.type.File.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info />

.SimulinkProject/Root.type.Files/STEPS.type.File/nano_hexapod.type.File/1.type.DIR_SIGNIFIER.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info />

.SimulinkProject/Root.type.ProjectPath/2e7b7a5d-07f5-4e60-bf5b-0686d86dbb21.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="STEPS/png" Type="Relative" />

.SimulinkProject/Root.type.ProjectPath/6cf1cee4-862b-45ea-9e86-a1762825df9e.type.Reference.xml

@@ -0,0 +1,2 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<Info Ref="STEPS/nano_hexapod" Type="Relative" />

src/initializeNanoHexapodFinal.m

@@ -1,6 +1,6 @@
-  function [nano_hexapod] = initializeNanoHexapodFinal(args)
+function [nano_hexapod] = initializeNanoHexapodFinal(args)
 
-  arguments
+arguments
     %% Bottom Flexible Joints
     args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
     args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
@@ -21,6 +21,8 @@
     args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Y bending Damping [Nm/(rad/s)]
     args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Torsionnal Damping [Nm/(rad/s)]
     args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*1e2 % Axial Damping [N/(m/s)]
+    %% Jacobian - Location of frame {A} and {B}
+    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
     %% Relative Motion Sensor
     args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
     %% Actuators
@@ -39,8 +41,8 @@
     args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
     args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
                                                                   % For Flexible
-      args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Sensor gain [V/m]
+    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
-  end
+end
 
 nano_hexapod = struct();
 
@@ -129,9 +131,9 @@ switch args.actuator_type
     nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
     nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
   case 'flexible'
-    nano_hexapod.actuator.K = readmatrix('APA300ML_full_mat_K.CSV'); % Stiffness Matrix
+    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
-    nano_hexapod.actuator.M = readmatrix('APA300ML_full_mat_M.CSV'); % Mass Matrix
+    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
-    nano_hexapod.actuator.P = extractNodes('APA300ML_full_out_nodes_3D.txt'); % Node coordiantes [m]
+    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
 end
 
 nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
@@ -139,6 +141,55 @@ nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
 nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
 nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
 
+nano_hexapod.geometry = struct();
+
+Fa = [[-86.05,  -74.78, 22.49],
+      [ 86.05,  -74.78, 22.49],
+      [ 107.79, -37.13, 22.49],
+      [ 21.74,  111.91, 22.49],
+      [-21.74,  111.91, 22.49],
+      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
+
+Mb = [[-28.47, -106.25, -22.50],
+      [ 28.47, -106.25, -22.50],
+      [ 106.25, 28.47,  -22.50],
+      [ 77.78,  77.78,  -22.50],
+      [-77.78,  77.78,  -22.50],
+      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
+
+Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
+
+si = Fb - Fa;
+si = si./vecnorm(si); % Normalize
+
+Fc = [[-29.362, -105.765, 52.605]
+      [ 29.362, -105.765, 52.605]
+      [ 106.276, 27.454,  52.605]
+      [ 76.914,  78.31,   52.605]
+      [-76.914,  78.31,   52.605]
+      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
+Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
+
+nano_hexapod.geometry.Fa = Fa;
+nano_hexapod.geometry.Fb = Fb;
+nano_hexapod.geometry.Fc = Fc;
+nano_hexapod.geometry.Mb = Mb;
+nano_hexapod.geometry.Mc = Mc;
+nano_hexapod.geometry.si = si;
+nano_hexapod.geometry.MO_B = args.MO_B;
+
+Bb = Mb - [0; 0; args.MO_B];
+
+nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
+
+switch args.motion_sensor_type
+  case 'struts'
+    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
+  case 'plates'
+    Bc = Mc - [0; 0; args.MO_B];
+    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
+end
+
 if nargout == 0
   save('./mat/stages.mat', 'nano_hexapod', '-append');
 end