#+TITLE: Simscape Model - Nano Active Stabilization System :DRAWER: #+LANGUAGE: en #+EMAIL: dehaeze.thomas@gmail.com #+AUTHOR: Dehaeze Thomas #+HTML_LINK_HOME: ../index.html #+HTML_LINK_UP: ../index.html #+HTML_HEAD: #+HTML_HEAD: #+BIND: org-latex-image-default-option "scale=1" #+BIND: org-latex-image-default-width "" #+LaTeX_CLASS: scrreprt #+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc] #+LATEX_HEADER: \input{preamble.tex} #+LATEX_HEADER_EXTRA: \input{preamble_extra.tex} #+LATEX_HEADER_EXTRA: \bibliography{simscape-nass.bib} #+BIND: org-latex-bib-compiler "biber" #+PROPERTY: header-args:matlab :session *MATLAB* #+PROPERTY: header-args:matlab+ :comments org #+PROPERTY: header-args:matlab+ :exports none #+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 #+PROPERTY: header-args:matlab+ :tangle no #+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}") #+PROPERTY: header-args:latex+ :imagemagick t :fit yes #+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150 #+PROPERTY: header-args:latex+ :imoutoptions -quality 100 #+PROPERTY: header-args:latex+ :results file raw replace #+PROPERTY: header-args:latex+ :buffer no #+PROPERTY: header-args:latex+ :tangle no #+PROPERTY: header-args:latex+ :eval no-export #+PROPERTY: header-args:latex+ :exports results #+PROPERTY: header-args:latex+ :mkdirp yes #+PROPERTY: header-args:latex+ :output-dir figs #+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") :END: #+latex: \clearpage * Build :noexport: #+NAME: startblock #+BEGIN_SRC emacs-lisp :results none :tangle no (add-to-list 'org-latex-classes '("scrreprt" "\\documentclass{scrreprt}" ("\\chapter{%s}" . "\\chapter*{%s}") ("\\section{%s}" . "\\section*{%s}") ("\\subsection{%s}" . "\\subsection*{%s}") ("\\paragraph{%s}" . "\\paragraph*{%s}") )) ;; Remove automatic org heading labels (defun my-latex-filter-removeOrgAutoLabels (text backend info) "Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings." (when (org-export-derived-backend-p backend 'latex) (replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text))) (add-to-list 'org-export-filter-headline-functions 'my-latex-filter-removeOrgAutoLabels) ;; Remove all org comments in the output LaTeX file (defun delete-org-comments (backend) (loop for comment in (reverse (org-element-map (org-element-parse-buffer) 'comment 'identity)) do (setf (buffer-substring (org-element-property :begin comment) (org-element-property :end comment)) ""))) (add-hook 'org-export-before-processing-hook 'delete-org-comments) ;; Use no package by default (setq org-latex-packages-alist nil) (setq org-latex-default-packages-alist nil) ;; Do not include the subtitle inside the title (setq org-latex-subtitle-separate t) (setq org-latex-subtitle-format "\\subtitle{%s}") (setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing org-ref-acronyms-before-parsing)) #+END_SRC * Notes :noexport: ** Notes Prefix is =nass= The goals of this report are: - [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors) - [X] ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...) - [ ] Determination of the *optimal stiffness* for the hexapod actuators: - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]] - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]] - [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]] - [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]] Should this be in this report? *This should be in chapter 2* - [X] Explain why HAC-LAC strategy is nice (*It was already explained in uniaxial model*) - [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control.org][different control architectures]] - [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org][hexapod - vibration isolation]] - [X] How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]]) - [X] ([[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org][decoupling-strategies]]): Decoupling strategies for HAC? (maybe also in previous report) *Will be in chapter 2* - [X] Validation of the concept using simulations: - [X] Find where this simulation in OL/CL is made (maybe for the conference?) It was re-made for micro-station validation. Will just have to do the same simulation but with nano-hexapod in closed-loop - Tomography experiment (maybe also Ty scans) - Open VS Closed loop results - *Conclusion*: concept validation nano hexapod architecture with APA decentralized IFF + centralized HAC - In this section simple control (in the frame of the struts) - Justify future used control architecture (control in the frame of the struts? Need to check what was done in ID31 tests) - Table that compares different approaches (specify performances in different DoF, same plans on the diagonal, etc...) - Literature review about Stewart platform control? *In chapter 2: Special section about MIMO control, complementary filters, etc...* ** Outline *** Control Kinematics - Explain how the position error can be expressed in the frame of the nano-hexapod - ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]): Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors) - Control architecture, block diagram *** LAC - How to apply/optimize IFF on an hexapod? ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]], [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]]) - Robustness to payload mass - Root Locus - Damping optimization *** HAC - ([[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]): Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...) - Determination of the *optimal stiffness* for the hexapod actuators: - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_optimal_stiffness.org][uncertainty_optimal_stiffness]] - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/optimal_stiffness_disturbances.org][optimal_stiffness_disturbances]] - [ ] [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][state-of-thesis-2020]] - [ ] [[file:/home/thomas/Cloud/meetings/group-meetings-me/2020-04-06-NASS-Design/2020-04-06-NASS-Design.org][group-meeting-optimal-stiffness]] - Effect of micro-station compliance - Effect of IFF - Effect of payload mass - Decoupled plant - Controller design *** Simulations - Take into account disturbances, metrology sensor noise. Maybe say here that we don't take in account other noise sources as they will be optimized latter (detail design phase) - Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]]) - Validation of concept ** DONE Old Outline CLOSED: [2024-11-07 Thu 16:19] *** Introduction :ignore: Discussion of: - Transformation matrices / control architecture (computation of the position error in the frame of the nano-hexapod) - Control of parallel architectures - Control in the frame of struts or cartesian? - Effect of rotation on IFF? => APA - HAC-LAC - New noise budgeting? *** Control Kinematics - Explain how the position error can be expressed in the frame of the nano-hexapod - block diagram - Explain how to go from external metrology to the frame of the nano-hexapod *** High Authority Control - Low Authority Control (HAC-LAC) - general idea - case for parallel manipulator: decentralized LAC + centralized HAC *** Decoupling Strategies for parallel manipulators [[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org::+TITLE: Diagonal control using the SVD and the Jacobian Matrix][study]] - Jacobian matrices, CoK, CoM, ... - Discussion of cubic architecture - SVD, Modal, ... *** Decentralized Integral Force Feedback (LAC) - Root Locus - Damping optimization *** Decoupled Dynamics - Centralized HAC - Control in the frame of the struts - Effect of IFF *** Centralized Position Controller (HAC) - Decoupled plant - Controller design *** Time domain simulations Goal: validation of the concept - Take into account disturbances, sensor noise, etc... - Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]]) ** DONE [#A] Merge the micro-station model with the nano-hexapod model CLOSED: [2025-02-12 Wed 12:10] SCHEDULED: <2025-02-12 Wed> - [X] *Start from the Simscape model of the ID31 tests* =/home/thomas/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/C5-test-bench-id31/matlab/nass_model_id31.slx= - [X] Remove LION metrology to have perfect measurement - [X] Remove nano-hexapod model and add simplified model - [ ] Add "cylindrical" payloads (configurable in mass) ** TODO [#B] Add payload configurable subsystem SCHEDULED: <2025-02-12 Wed> ** TODO [#B] Discuss the necessity of estimated Rz? One big advantage of doing the control in the cartesian plane, is that we don't need the estimation of nano-hexapod Rz, therefore we don't need the encoders anymore! Maybe this should be done *here*. Here it can be reminded when doing the control in the cartesian frame. ** TODO [#B] Check if things are compatible to results of uniaxial model ** CANC [#C] What performance metric can we use? :@christophe: CLOSED: [2024-11-12 Tue 09:22] - State "CANC" from "QUES" [2024-11-12 Tue 09:22] This can be nice to have a (scalar) performance metric that can be used for optimization. In cite:hauge04_sensor_contr_space_based_six, a (scalar) performance metric representing the 6dof transmissibility is used. ** DONE [#C] Identify the sensibility to disturbances without the nano-hexapod and save the results CLOSED: [2024-11-07 Thu 09:20] This can then be used to compare with obtained performance with the nano-hexapod. This should be done in the ustation report (A4). * Introduction :ignore: From last sections: - Uniaxial: No stiff nano-hexapod (should also demonstrate that here) - Rotating: No soft nano-hexapod, Decentralized IFF can be used robustly by adding parallel stiffness In this section: - Take the model of the nano-hexapod with stiffness 1um/N - Apply decentralized IFF - Apply HAC-LAC - Check robustness to payload change - Simulation of experiments * Control Kinematics :PROPERTIES: :HEADER-ARGS:matlab+: :tangle matlab/nass_1_kinematics.m :END: <> ** Introduction :ignore: - Explain how the position error can be expressed in the frame of the nano-hexapod - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/positioning_error.org][positioning_error]]: Explain how the NASS control is made (computation of the wanted position, measurement of the sample position, computation of the errors) - Control architecture, block diagram - Schematic with micro-station + nass + metrology + control system - Zoom in the control system with blocs - Then explain all the blocs - Say that there are many control strategies. It will be the topic of chapter 2.3. Here, we start with something simple: control in the frame of the struts ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** Micro Station Kinematics - from ref:ssec:ustation_kinematics, computation of the wanted sample pose from the setpoint of each stage. ** Computation of the sample's pose error From metrology (here supposed to be perfect 6-DoF), compute the sample's pose error. Has to invert the homogeneous transformation. ** Position error in the frame of the nano-hexapod Explain how to compute the errors in the frame of the struts (rotating) * Decentralized Active Damping :PROPERTIES: :HEADER-ARGS:matlab+: :tangle matlab/nass_2_active_damping.m :END: <> ** Introduction :ignore: - How to apply/optimize IFF on an hexapod? () - Robustness to payload mass - Root Locus - Damping optimization - [ ][[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/control_active_damping.org][control_active_damping]] - [ ][[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-active-damping.org][active damping for stewart platforms]] - [ ][[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org::*Vibration Control and Active Damping][Vibration Control and Active Damping]] ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** IFF Plant #+begin_src matlab %% Identify the plant dynamics using the Simscape model % Initialize each Simscape model elements initializeGround(); initializeGranite(); initializeTy(); initializeRy(); initializeRz(); initializeMicroHexapod(); initializeSimplifiedNanoHexapod(); initializeSample('type', 'cylindrical'); initializeSimscapeConfiguration('gravity', false); initializeDisturbances('enable', false); initializeLoggingConfiguration('log', 'none'); initializeController('type', 'open-loop'); initializeReferences(); % Input/Output definition % clear io; io_i = 1; % io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [V] % io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Vs'); io_i = io_i + 1; % Force Sensors voltages [V] % io(io_i) = linio([mdl, '/Tracking Error'], 1, 'openoutput', [], 'EdL'); io_i = io_i + 1; % Position Errors [m] % % With no payload % Gm = exp(-1e-4*s)*linearize(mdl, io); % Gm.InputName = {'u1', 'u2', 'u3', 'u4', 'u5', 'u6'}; % Gm.OutputName = {'Vs1', 'Vs2', 'Vs3', 'Vs4', 'Vs5', 'Vs6', ... % 'eL1', 'eL2', 'eL3', 'eL4', 'eL5', 'eL6'}; #+end_src - Show how it changes with the payload mass (1, 25, 50) - Effect of rotation (1rpm, 60rpm) ** Controller Design - Apply IFF - Show Root Locus - Choose optimal gain. Here in MIMO, cannot have optimal damping for all modes. (there is a paper that tries to optimize that) - Show robustness to change of payload (loci?) - Reference to paper showing stability in MIMO for decentralized IFF ** Sensitivity to disturbances - Compute transfer functions from spindle vertical error to sample vertical error with IFF (and compare without the NASS) - Same for horizontal - Maybe noise budgeting, but may be complex in MIMO... * Centralized Active Vibration Control :PROPERTIES: :HEADER-ARGS:matlab+: :tangle matlab/nass_3_hac.m :END: <> ** Introduction :ignore: - [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/uncertainty_experiment.org][uncertainty_experiment]]: Effect of experimental conditions on the plant (payload mass, Ry position, Rz position, Rz velocity, etc...) - Effect of micro-station compliance - Effect of IFF - Effect of payload mass - Decoupled plant - Controller design From control kinematics: - Talk about issue of not estimating Rz from external metrology? (maybe could be nice to discuss that during the experiments!) - Show what happens is Rz is not estimated (for instance supposed equaled to zero => increased coupling) ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** HAC Plant - Compute transfer function from u to dL (with IFF applied) ** Effect of Payload mass - Show effect of payload mass + rotation ** Controller design - Show robustness with Loci ** Sensitivity to disturbances - Compute transfer functions from spindle vertical error to sample vertical error with HAC-IFF Compare without the NASS, and with just IFF - Same for horizontal - Maybe noise budgeting, but may be complex in MIMO... ** Tomography experiment - With HAC-IFF, perform tomography experiment, and compare with open-loop - Take into account disturbances, metrology sensor noise. Maybe say here that we don't take in account other noise sources as they will be optimized latter (detail design phase) - Tomography + lateral scans (same as what was done in open loop [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A4-simscape-micro-station/simscape-micro-station.org::*Simulation of Scientific Experiments][here]]) - Validation of concept * Conclusion <> * Bibliography :ignore: #+latex: \printbibliography[heading=bibintoc,title={Bibliography}] * Matlab Functions :noexport: ** =initializeSimscapeConfiguration=: Simscape Configuration :PROPERTIES: :header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: *** Function description #+begin_src matlab function [] = initializeSimscapeConfiguration(args) #+end_src *** Optional Parameters #+begin_src matlab arguments args.gravity logical {mustBeNumericOrLogical} = true end #+end_src *** Structure initialization #+begin_src matlab conf_simscape = struct(); #+end_src *** Add Type #+begin_src matlab if args.gravity conf_simscape.type = 1; else conf_simscape.type = 2; end #+end_src *** Save the Structure #+begin_src matlab if exist('./mat', 'dir') if exist('./mat/nass_model_conf_simscape.mat', 'file') save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append'); else save('mat/nass_model_conf_simscape.mat', 'conf_simscape'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file') save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append'); else save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape'); end end #+end_src ** =initializeLoggingConfiguration=: Logging Configuration :PROPERTIES: :header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: *** Function description #+begin_src matlab function [] = initializeLoggingConfiguration(args) #+end_src *** Optional Parameters #+begin_src matlab arguments args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none' args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 end #+end_src *** Structure initialization #+begin_src matlab conf_log = struct(); #+end_src *** Add Type #+begin_src matlab switch args.log case 'none' conf_log.type = 0; case 'all' conf_log.type = 1; case 'forces' conf_log.type = 2; end #+end_src *** Sampling Time #+begin_src matlab conf_log.Ts = args.Ts; #+end_src *** Save the Structure #+begin_src matlab if exist('./mat', 'dir') if exist('./mat/nass_model_conf_log.mat', 'file') save('mat/nass_model_conf_log.mat', 'conf_log', '-append'); else save('mat/nass_model_conf_log.mat', 'conf_log'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_conf_log.mat', 'file') save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append'); else save('matlab/mat/nass_model_conf_log.mat', 'conf_log'); end end #+end_src ** =initializeReferences=: Generate Reference Signals :PROPERTIES: :header-args:matlab+: :tangle matlab/src/initializeReferences.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: *** Function Declaration and Documentation #+begin_src matlab function [ref] = initializeReferences(args) #+end_src *** Optional Parameters #+begin_src matlab arguments % Sampling Frequency [s] args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Maximum simulation time [s] args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100 % Either "constant" / "triangular" / "sinusoidal" args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' % Amplitude of the displacement [m] args.Dy_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the displacement [s] args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % Either "constant" / "triangular" / "sinusoidal" args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant' % Amplitude [rad] args.Ry_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the displacement [s] args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % Either "constant" / "rotating" args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant' % Initial angle [rad] args.Rz_amplitude (1,1) double {mustBeNumeric} = 0 % Period of the rotating [s] args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1 % For now, only constant is implemented args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant' % Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles) args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ... % For now, only constant is implemented args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant' % Initial position of the two masses args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi] % For now, only constant is implemented args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant' % Initial position [m,m,m,rad,rad,rad] of the top platform args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1) end #+end_src *** Initialize Parameters #+begin_src matlab %% Set Sampling Time Ts = args.Ts; Tmax = args.Tmax; %% Low Pass Filter to filter out the references s = zpk('s'); w0 = 2*pi*10; xi = 1; H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2); #+end_src *** Translation Stage #+begin_src matlab %% Translation stage - Dy t = 0:Ts:Tmax; % Time Vector [s] Dy = zeros(length(t), 1); Dyd = zeros(length(t), 1); Dydd = zeros(length(t), 1); switch args.Dy_type case 'constant' Dy(:) = args.Dy_amplitude; Dyd(:) = 0; Dydd(:) = 0; case 'triangular' % This is done to unsure that we start with no displacement Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2); i0 = find(t>=args.Dy_period/4,1); Dy(1:end-i0+1) = Dy_raw(i0:end); Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value % The signal is filtered out Dy = lsim(H_lpf, Dy, t); Dyd = lsim(H_lpf*s, Dy, t); Dydd = lsim(H_lpf*s^2, Dy, t); case 'sinusoidal' Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t); Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t); Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t); otherwise warning('Dy_type is not set correctly'); end Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd); #+end_src *** Tilt Stage #+begin_src matlab %% Tilt Stage - Ry t = 0:Ts:Tmax; % Time Vector [s] Ry = zeros(length(t), 1); Ryd = zeros(length(t), 1); Rydd = zeros(length(t), 1); switch args.Ry_type case 'constant' Ry(:) = args.Ry_amplitude; Ryd(:) = 0; Rydd(:) = 0; case 'triangular' Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2); i0 = find(t>=args.Ry_period/4,1); Ry(1:end-i0+1) = Ry_raw(i0:end); Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value % The signal is filtered out Ry = lsim(H_lpf, Ry, t); Ryd = lsim(H_lpf*s, Ry, t); Rydd = lsim(H_lpf*s^2, Ry, t); case 'sinusoidal' Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t); Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t); Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t); otherwise warning('Ry_type is not set correctly'); end Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd); #+end_src *** Spindle #+begin_src matlab %% Spindle - Rz t = 0:Ts:Tmax; % Time Vector [s] Rz = zeros(length(t), 1); Rzd = zeros(length(t), 1); Rzdd = zeros(length(t), 1); switch args.Rz_type case 'constant' Rz(:) = args.Rz_amplitude; Rzd(:) = 0; Rzdd(:) = 0; case 'rotating-not-filtered' Rz(:) = 2*pi/args.Rz_period*t; % The signal is filtered out Rz(:) = 2*pi/args.Rz_period*t; Rzd(:) = 2*pi/args.Rz_period; Rzdd(:) = 0; % We add the angle offset Rz = Rz + args.Rz_amplitude; case 'rotating' Rz(:) = 2*pi/args.Rz_period*t; % The signal is filtered out Rz = lsim(H_lpf, Rz, t); Rzd = lsim(H_lpf*s, Rz, t); Rzdd = lsim(H_lpf*s^2, Rz, t); % We add the angle offset Rz = Rz + args.Rz_amplitude; otherwise warning('Rz_type is not set correctly'); end Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd); #+end_src *** Micro Hexapod #+begin_src matlab %% Micro-Hexapod t = [0, Ts]; Dh = zeros(length(t), 6); Dhl = zeros(length(t), 6); switch args.Dh_type case 'constant' Dh = [args.Dh_pos, args.Dh_pos]; load('nass_model_stages.mat', 'micro_hexapod'); AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)]; tx = args.Dh_pos(4); ty = args.Dh_pos(5); tz = args.Dh_pos(6); ARB = [cos(tz) -sin(tz) 0; sin(tz) cos(tz) 0; 0 0 1]*... [ cos(ty) 0 sin(ty); 0 1 0; -sin(ty) 0 cos(ty)]*... [1 0 0; 0 cos(tx) -sin(tx); 0 sin(tx) cos(tx)]; [~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB); Dhl = [Dhl, Dhl]; otherwise warning('Dh_type is not set correctly'); end Dh = struct('time', t, 'signals', struct('values', Dh)); Dhl = struct('time', t, 'signals', struct('values', Dhl)); #+end_src *** Save the Structure #+begin_src matlab if exist('./mat', 'dir') if exist('./mat/nass_model_references.mat', 'file') save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append'); else save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_references.mat', 'file') save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append'); else save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts'); end end #+end_src ** =initializeDisturbances=: Initialize Disturbances :PROPERTIES: :header-args:matlab+: :tangle matlab/src/initializeDisturbances.m :header-args:matlab+: :comments none :mkdirp yes :header-args:matlab+: :eval no :results none :END: *** Function Declaration and Documentation #+begin_src matlab function [] = initializeDisturbances(args) % initializeDisturbances - Initialize the disturbances % % Syntax: [] = initializeDisturbances(args) % % Inputs: % - args - #+end_src *** Optional Parameters #+begin_src matlab arguments % Global parameter to enable or disable the disturbances args.enable logical {mustBeNumericOrLogical} = true % Ground Motion - X direction args.Dw_x logical {mustBeNumericOrLogical} = true % Ground Motion - Y direction args.Dw_y logical {mustBeNumericOrLogical} = true % Ground Motion - Z direction args.Dw_z logical {mustBeNumericOrLogical} = true % Translation Stage - X direction args.Fdy_x logical {mustBeNumericOrLogical} = true % Translation Stage - Z direction args.Fdy_z logical {mustBeNumericOrLogical} = true % Spindle - X direction args.Frz_x logical {mustBeNumericOrLogical} = true % Spindle - Y direction args.Frz_y logical {mustBeNumericOrLogical} = true % Spindle - Z direction args.Frz_z logical {mustBeNumericOrLogical} = true end #+end_src #+begin_src matlab % Initialization of random numbers rng("shuffle"); #+end_src *** Ground Motion #+begin_src matlab %% Ground Motion if args.enable % Load the PSD of disturbance load('ustation_disturbance_psd.mat', 'gm_dist') % Frequency Data Dw.f = gm_dist.f; Dw.psd_x = gm_dist.pxx_x; Dw.psd_y = gm_dist.pxx_y; Dw.psd_z = gm_dist.pxx_z; % Time data Fs = 2*Dw.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] N = 2*length(Dw.f); % Number of Samples match the one of the wanted PSD T0 = N/Fs; % Signal Duration [s] Dw.t = linspace(0, T0, N+1)'; % Time Vector [s] % ASD representation of the ground motion C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(Dw.psd_x(i)/T0); end if args.Dw_x theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m] else Dw.x = zeros(length(Dw.t), 1); end if args.Dw_y theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m] else Dw.y = zeros(length(Dw.t), 1); end if args.Dw_y theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m] else Dw.z = zeros(length(Dw.t), 1); end else Dw.t = [0,1]; % Time Vector [s] Dw.x = [0,0]; % Ground Motion - X [m] Dw.y = [0,0]; % Ground Motion - Y [m] Dw.z = [0,0]; % Ground Motion - Z [m] end #+end_src *** Translation stage #+begin_src matlab %% Translation stage if args.enable % Load the PSD of disturbance load('ustation_disturbance_psd.mat', 'dy_dist') % Frequency Data Dy.f = dy_dist.f; Dy.psd_x = dy_dist.pxx_fx; Dy.psd_z = dy_dist.pxx_fz; % Time data Fs = 2*Dy.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] N = 2*length(Dy.f); % Number of Samples match the one of the wanted PSD T0 = N/Fs; % Signal Duration [s] Dy.t = linspace(0, T0, N+1)'; % Time Vector [s] % ASD representation of the disturbance voice C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(Dy.psd_x(i)/T0); end if args.Fdy_x theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N] else Dy.x = zeros(length(Dy.t), 1); end if args.Fdy_z theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N] else Dy.z = zeros(length(Dy.t), 1); end else Dy.t = [0,1]; % Time Vector [s] Dy.x = [0,0]; % Translation Stage disturbances - X [N] Dy.z = [0,0]; % Translation Stage disturbances - Z [N] end #+end_src *** Spindle #+begin_src matlab %% Spindle if args.enable % Load the PSD of disturbance load('ustation_disturbance_psd.mat', 'rz_dist') % Frequency Data Rz.f = rz_dist.f; Rz.psd_x = rz_dist.pxx_fx; Rz.psd_y = rz_dist.pxx_fy; Rz.psd_z = rz_dist.pxx_fz; % Time data Fs = 2*Rz.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz] N = 2*length(Rz.f); % Number of Samples match the one of the wanted PSD T0 = N/Fs; % Signal Duration [s] Rz.t = linspace(0, T0, N+1)'; % Time Vector [s] % ASD representation of the disturbance voice C = zeros(N/2,1); for i = 1:N/2 C(i) = sqrt(Rz.psd_x(i)/T0); end if args.Frz_x theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N] else Rz.x = zeros(length(Rz.t), 1); end if args.Frz_y theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N] else Rz.y = zeros(length(Rz.t), 1); end if args.Frz_z theta = 2*pi*rand(N/2,1); % Generate random phase [rad] Cx = [0 ; C.*complex(cos(theta),sin(theta))]; Cx = [Cx; flipud(conj(Cx(2:end)))];; Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N] else Rz.z = zeros(length(Rz.t), 1); end else Rz.t = [0,1]; % Time Vector [s] Rz.x = [0,0]; % Spindle disturbances - X [N] Rz.y = [0,0]; % Spindle disturbances - X [N] Rz.z = [0,0]; % Spindle disturbances - Z [N] end #+end_src *** Direct Forces #+begin_src matlab u = zeros(100, 6); Fd = u; #+end_src *** Set initial value to zero #+begin_src matlab Dw.x = Dw.x - Dw.x(1); Dw.y = Dw.y - Dw.y(1); Dw.z = Dw.z - Dw.z(1); Dy.x = Dy.x - Dy.x(1); Dy.z = Dy.z - Dy.z(1); Rz.x = Rz.x - Rz.x(1); Rz.y = Rz.y - Rz.y(1); Rz.z = Rz.z - Rz.z(1); #+end_src *** Save the Structure #+begin_src matlab if exist('./mat', 'dir') save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args'); elseif exist('./matlab', 'dir') save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args'); end #+end_src ** =initializeController=: Initialize Controller #+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no function [] = initializeController(args) arguments args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop' end controller = struct(); switch args.type case 'open-loop' controller.type = 1; controller.name = 'Open-Loop'; case 'dvf' controller.type = 2; controller.name = 'Decentralized Direct Velocity Feedback'; case 'iff' controller.type = 3; controller.name = 'Decentralized Integral Force Feedback'; case 'hac-dvf' controller.type = 4; controller.name = 'HAC-DVF'; case 'ref-track-L' controller.type = 5; controller.name = 'Reference Tracking in the frame of the legs'; case 'ref-track-iff-L' controller.type = 6; controller.name = 'Reference Tracking in the frame of the legs + IFF'; case 'cascade-hac-lac' controller.type = 7; controller.name = 'Cascade Control + HAC-LAC'; case 'hac-iff' controller.type = 8; controller.name = 'HAC-IFF'; case 'stabilizing' controller.type = 9; controller.name = 'Stabilizing Controller'; end if exist('./mat', 'dir') save('mat/nass_model_controller.mat', 'controller'); elseif exist('./matlab', 'dir') save('matlab/mat/nass_model_controller.mat', 'controller'); end end #+end_src ** =describeMicroStationSetup= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/describeMicroStationSetup.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: *** Function description #+begin_src matlab function [] = describeMicroStationSetup() % describeMicroStationSetup - % % Syntax: [] = describeMicroStationSetup() % % Inputs: % - - % % Outputs: % - - #+end_src *** Simscape Configuration #+begin_src matlab load('./mat/nass_model_conf_simscape.mat', 'conf_simscape'); #+end_src #+begin_src matlab fprintf('Simscape Configuration:\n'); if conf_simscape.type == 1 fprintf('- Gravity is included\n'); else fprintf('- Gravity is not included\n'); end fprintf('\n'); #+end_src *** Disturbances #+begin_src matlab load('./mat/nass_model_disturbances.mat', 'args'); #+end_src #+begin_src matlab fprintf('Disturbances:\n'); if ~args.enable fprintf('- No disturbance is included\n'); else if args.Dwx && args.Dwy && args.Dwz fprintf('- Ground motion\n'); end if args.Fdy_x && args.Fdy_z fprintf('- Vibrations of the Translation Stage\n'); end if args.Frz_z fprintf('- Vibrations of the Spindle\n'); end end fprintf('\n'); #+end_src *** References #+begin_src matlab load('./mat/nass_model_references.mat', 'args'); #+end_src #+begin_src matlab fprintf('Reference Tracking:\n'); fprintf('- Translation Stage:\n'); switch args.Dy_type case 'constant' fprintf(' - Constant Position\n'); fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3); case 'triangular' fprintf(' - Triangular Path\n'); fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3); fprintf(' - Period = %.0f [s]\n', args.Dy_period); case 'sinusoidal' fprintf(' - Sinusoidal Path\n'); fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3); fprintf(' - Period = %.0f [s]\n', args.Dy_period); end fprintf('- Tilt Stage:\n'); switch args.Ry_type case 'constant' fprintf(' - Constant Position\n'); fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3); case 'triangular' fprintf(' - Triangular Path\n'); fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3); fprintf(' - Period = %.0f [s]\n', args.Ry_period); case 'sinusoidal' fprintf(' - Sinusoidal Path\n'); fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3); fprintf(' - Period = %.0f [s]\n', args.Ry_period); end fprintf('- Spindle:\n'); switch args.Rz_type case 'constant' fprintf(' - Constant Position\n'); fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude); case { 'rotating', 'rotating-not-filtered' } fprintf(' - Rotating\n'); fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period); end fprintf('- Micro Hexapod:\n'); switch args.Dh_type case 'constant' fprintf(' - Constant Position\n'); fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3)); fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6)); end fprintf('\n'); #+end_src *** Micro-Station #+begin_src matlab load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc'); #+end_src #+begin_src matlab fprintf('Micro Station:\n'); if granite.type == 1 && ... ty.type == 1 && ... ry.type == 1 && ... rz.type == 1 && ... micro_hexapod.type == 1; fprintf('- All stages are rigid\n'); elseif granite.type == 2 && ... ty.type == 2 && ... ry.type == 2 && ... rz.type == 2 && ... micro_hexapod.type == 2; fprintf('- All stages are flexible\n'); else if granite.type == 1 || granite.type == 4 fprintf('- Granite is rigid\n'); else fprintf('- Granite is flexible\n'); end if ty.type == 1 || ty.type == 4 fprintf('- Translation Stage is rigid\n'); else fprintf('- Translation Stage is flexible\n'); end if ry.type == 1 || ry.type == 4 fprintf('- Tilt Stage is rigid\n'); else fprintf('- Tilt Stage is flexible\n'); end if rz.type == 1 || rz.type == 4 fprintf('- Spindle is rigid\n'); else fprintf('- Spindle is flexible\n'); end if micro_hexapod.type == 1 || micro_hexapod.type == 4 fprintf('- Micro Hexapod is rigid\n'); else fprintf('- Micro Hexapod is flexible\n'); end end fprintf('\n'); #+end_src ** =computeReferencePose= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/computeReferencePose.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: #+begin_src matlab function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) % computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample % % Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn) % % Inputs: % - Dy - Reference of the Translation Stage [m] % - Ry - Reference of the Tilt Stage [rad] % - Rz - Reference of the Spindle [rad] % - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad] % - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad] % % Outputs: % - WTr - %% Translation Stage Rty = [1 0 0 0; 0 1 0 Dy; 0 0 1 0; 0 0 0 1]; %% Tilt Stage - Pure rotating aligned with Ob Rry = [ cos(Ry) 0 sin(Ry) 0; 0 1 0 0; -sin(Ry) 0 cos(Ry) 0; 0 0 0 1]; %% Spindle - Rotation along the Z axis Rrz = [cos(Rz) -sin(Rz) 0 0 ; sin(Rz) cos(Rz) 0 0 ; 0 0 1 0 ; 0 0 0 1 ]; %% Micro-Hexapod Rhx = [1 0 0; 0 cos(Dh(4)) -sin(Dh(4)); 0 sin(Dh(4)) cos(Dh(4))]; Rhy = [ cos(Dh(5)) 0 sin(Dh(5)); 0 1 0; -sin(Dh(5)) 0 cos(Dh(5))]; Rhz = [cos(Dh(6)) -sin(Dh(6)) 0; sin(Dh(6)) cos(Dh(6)) 0; 0 0 1]; Rh = [1 0 0 Dh(1) ; 0 1 0 Dh(2) ; 0 0 1 Dh(3) ; 0 0 0 1 ]; Rh(1:3, 1:3) = Rhz*Rhy*Rhx; %% Nano-Hexapod Rnx = [1 0 0; 0 cos(Dn(4)) -sin(Dn(4)); 0 sin(Dn(4)) cos(Dn(4))]; Rny = [ cos(Dn(5)) 0 sin(Dn(5)); 0 1 0; -sin(Dn(5)) 0 cos(Dn(5))]; Rnz = [cos(Dn(6)) -sin(Dn(6)) 0; sin(Dn(6)) cos(Dn(6)) 0; 0 0 1]; Rn = [1 0 0 Dn(1) ; 0 1 0 Dn(2) ; 0 0 1 Dn(3) ; 0 0 0 1 ]; Rn(1:3, 1:3) = Rnz*Rny*Rnx; %% Total Homogeneous transformation WTr = Rty*Rry*Rrz*Rh*Rn; end #+end_src ** =circlefit= :PROPERTIES: :header-args:matlab+: :tangle matlab/src/circlefit.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: #+begin_src matlab function [xc,yc,R,a] = circlefit(x,y) % % [xc yx R] = circfit(x,y) % % fits a circle in x,y plane in a more accurate % (less prone to ill condition ) % procedure than circfit2 but using more memory % x,y are column vector where (x(i),y(i)) is a measured point % % result is center point (yc,xc) and radius R % an optional output is the vector of coeficient a % describing the circle's equation % % x^2+y^2+a(1)*x+a(2)*y+a(3)=0 % % By: Izhak bucher 25/oct /1991, x=x(:); y=y(:); a=[x y ones(size(x))]\[-(x.^2+y.^2)]; xc = -.5*a(1); yc = -.5*a(2); R = sqrt((a(1)^2+a(2)^2)/4-a(3)); #+end_src ** Initialize Micro-Station Stages *** =initializeGround=: Ground #+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no function [ground] = initializeGround(args) arguments args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid' args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m] end ground = struct(); switch args.type case 'none' ground.type = 0; case 'rigid' ground.type = 1; end ground.shape = [2, 2, 0.5]; % [m] ground.density = 2800; % [kg/m3] ground.rot_point = args.rot_point; if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'ground', '-append'); else save('mat/nass_model_stages.mat', 'ground'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'ground', '-append'); else save('matlab/mat/nass_model_stages.mat', 'ground'); end end end #+end_src *** =initializeGranite=: Granite #+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no function [granite] = initializeGranite(args) arguments args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible' args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3] args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m] args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [N/(m/s)] args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m] args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m] args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m] args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m] end granite = struct(); switch args.type case 'none' granite.type = 0; case 'rigid' granite.type = 1; case 'flexible' granite.type = 2; end granite.density = args.density; % [kg/m3] granite.STEP = 'granite.STEP'; % Z-offset for the initial position of the sample with respect to the granite top surface. granite.sample_pos = args.sample_pos; % [m] granite.K = args.K; % [N/m] granite.C = args.C; % [N/(m/s)] if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'granite', '-append'); else save('mat/nass_model_stages.mat', 'granite'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'granite', '-append'); else save('matlab/mat/nass_model_stages.mat', 'granite'); end end end #+end_src *** =initializeTy=: Translation Stage #+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no function [ty] = initializeTy(args) arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' end ty = struct(); switch args.type case 'none' ty.type = 0; case 'rigid' ty.type = 1; case 'flexible' ty.type = 2; end % Ty Granite frame ty.granite_frame.density = 7800; % [kg/m3] => 43kg ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP'; % Guide Translation Ty ty.guide.density = 7800; % [kg/m3] => 76kg ty.guide.STEP = 'Ty_Guide.STEP'; % Ty - Guide_Translation12 ty.guide12.density = 7800; % [kg/m3] ty.guide12.STEP = 'Ty_Guide_12.STEP'; % Ty - Guide_Translation11 ty.guide11.density = 7800; % [kg/m3] ty.guide11.STEP = 'Ty_Guide_11.STEP'; % Ty - Guide_Translation22 ty.guide22.density = 7800; % [kg/m3] ty.guide22.STEP = 'Ty_Guide_22.STEP'; % Ty - Guide_Translation21 ty.guide21.density = 7800; % [kg/m3] ty.guide21.STEP = 'Ty_Guide_21.STEP'; % Ty - Plateau translation ty.frame.density = 7800; % [kg/m3] ty.frame.STEP = 'Ty_Stage.STEP'; % Ty Stator Part ty.stator.density = 5400; % [kg/m3] ty.stator.STEP = 'Ty_Motor_Stator.STEP'; % Ty Rotor Part ty.rotor.density = 5400; % [kg/m3] ty.rotor.STEP = 'Ty_Motor_Rotor.STEP'; ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad] ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)] if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'ty', '-append'); else save('mat/nass_model_stages.mat', 'ty'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'ty', '-append'); else save('matlab/mat/nass_model_stages.mat', 'ty'); end end end #+end_src *** =initializeRy=: Tilt Stage #+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no function [ry] = initializeRy(args) arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' args.Ry_init (1,1) double {mustBeNumeric} = 0 end ry = struct(); switch args.type case 'none' ry.type = 0; case 'rigid' ry.type = 1; case 'flexible' ry.type = 2; end % Ry - Guide for the tilt stage ry.guide.density = 7800; % [kg/m3] ry.guide.STEP = 'Tilt_Guide.STEP'; % Ry - Rotor of the motor ry.rotor.density = 2400; % [kg/m3] ry.rotor.STEP = 'Tilt_Motor_Axis.STEP'; % Ry - Motor ry.motor.density = 3200; % [kg/m3] ry.motor.STEP = 'Tilt_Motor.STEP'; % Ry - Plateau Tilt ry.stage.density = 7800; % [kg/m3] ry.stage.STEP = 'Tilt_Stage.STEP'; % Z-Offset so that the center of rotation matches the sample center; ry.z_offset = 0.58178; % [m] ry.Ry_init = args.Ry_init; % [rad] ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8]; ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4]; if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'ry', '-append'); else save('mat/nass_model_stages.mat', 'ry'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'ry', '-append'); else save('matlab/mat/nass_model_stages.mat', 'ry'); end end end #+end_src *** =initializeRz=: Spindle #+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no function [rz] = initializeRz(args) arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' end rz = struct(); switch args.type case 'none' rz.type = 0; case 'rigid' rz.type = 1; case 'flexible' rz.type = 2; end % Spindle - Slip Ring rz.slipring.density = 7800; % [kg/m3] rz.slipring.STEP = 'Spindle_Slip_Ring.STEP'; % Spindle - Rotor rz.rotor.density = 7800; % [kg/m3] rz.rotor.STEP = 'Spindle_Rotor.STEP'; % Spindle - Stator rz.stator.density = 7800; % [kg/m3] rz.stator.STEP = 'Spindle_Stator.STEP'; rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7]; rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4]; if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'rz', '-append'); else save('mat/nass_model_stages.mat', 'rz'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'rz', '-append'); else save('matlab/mat/nass_model_stages.mat', 'rz'); end end end #+end_src *** =initializeMicroHexapod=: Micro Hexapod #+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no function [micro_hexapod] = initializeMicroHexapod(args) arguments args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible' % initializeFramesPositions args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 args.MO_B (1,1) double {mustBeNumeric} = 270e-3 % generateGeneralConfiguration args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-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} = 45e-3 args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3 args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180) % initializeStrutDynamics args.Ki (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e7 args.Ci (1,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3 % initializeCylindricalPlatforms args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10 args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3 args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10 args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3 args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % initializeCylindricalStruts args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1 args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1 args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3 % inverseKinematics args.AP (3,1) double {mustBeNumeric} = zeros(3,1) args.ARB (3,3) double {mustBeNumeric} = eye(3) end stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, ... 'H', args.H, ... 'MO_B', args.MO_B); stewart = generateGeneralConfiguration(stewart, ... 'FH', args.FH, ... 'FR', args.FR, ... 'FTh', args.FTh, ... 'MH', args.MH, ... 'MR', args.MR, ... 'MTh', args.MTh); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, ... 'k', args.Ki, ... 'c', args.Ci); stewart = initializeJointDynamics(stewart, ... 'type_F', '2dof', ... 'type_M', '3dof'); stewart = initializeCylindricalPlatforms(stewart, ... 'Fpm', args.Fpm, ... 'Fph', args.Fph, ... 'Fpr', args.Fpr, ... 'Mpm', args.Mpm, ... 'Mph', args.Mph, ... 'Mpr', args.Mpr); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', args.Fsm, ... 'Fsh', args.Fsh, ... 'Fsr', args.Fsr, ... 'Msm', args.Msm, ... 'Msh', args.Msh, ... 'Msr', args.Msr); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart, ... 'AP', args.AP, ... 'ARB', args.ARB); stewart = initializeInertialSensor(stewart, 'type', 'none'); switch args.type case 'none' stewart.type = 0; case 'rigid' stewart.type = 1; case 'flexible' stewart.type = 2; end micro_hexapod = stewart; if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'micro_hexapod', '-append'); else save('mat/nass_model_stages.mat', 'micro_hexapod'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append'); else save('matlab/mat/nass_model_stages.mat', 'micro_hexapod'); end end end #+end_src *** =initializeSimplifiedNanoHexapod=: Nano Hexapod #+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no function [nano_hexapod] = initializeSimplifiedNanoHexapod(args) arguments %% initializeFramesPositions args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m] args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m] %% generateGeneralConfiguration args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m] args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m] args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad] args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m] args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m] args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad] %% Actuators args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof' args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6 args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4 args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605 args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302 args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50 args.actuator_cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100 args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50 %% initializeCylindricalPlatforms args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg] args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m] args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m] args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg] args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m] args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m] %% initializeCylindricalStruts args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg] args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m] args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg] args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m] args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m] %% Bottom and Top Flexible Joints args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 %% inverseKinematics args.AP (3,1) double {mustBeNumeric} = zeros(3,1) args.ARB (3,3) double {mustBeNumeric} = eye(3) end stewart = initializeStewartPlatform(); stewart = initializeFramesPositions(stewart, ... 'H', args.H, ... 'MO_B', args.MO_B); stewart = generateGeneralConfiguration(stewart, ... 'FH', args.FH, ... 'FR', args.FR, ... 'FTh', args.FTh, ... 'MH', args.MH, ... 'MR', args.MR, ... 'MTh', args.MTh); stewart = computeJointsPose(stewart); stewart = initializeStrutDynamics(stewart, ... 'type', args.actuator_type, ... 'k', args.actuator_k, ... 'kp', args.actuator_kp, ... 'ke', args.actuator_ke, ... 'ka', args.actuator_ka, ... 'c', args.actuator_c, ... 'cp', args.actuator_cp, ... 'ce', args.actuator_ce, ... 'ca', args.actuator_ca); stewart = initializeJointDynamics(stewart, ... 'type_F', args.flex_type_F, ... 'type_M', args.flex_type_M, ... 'Kf_M', args.Kf_M, ... 'Cf_M', args.Cf_M, ... 'Kt_M', args.Kt_M, ... 'Ct_M', args.Ct_M, ... 'Kf_F', args.Kf_F, ... 'Cf_F', args.Cf_F, ... 'Kt_F', args.Kt_F, ... 'Ct_F', args.Ct_F, ... 'Ka_F', args.Ka_F, ... 'Ca_F', args.Ca_F, ... 'Kr_F', args.Kr_F, ... 'Cr_F', args.Cr_F, ... 'Ka_M', args.Ka_M, ... 'Ca_M', args.Ca_M, ... 'Kr_M', args.Kr_M, ... 'Cr_M', args.Cr_M); stewart = initializeCylindricalPlatforms(stewart, ... 'Fpm', args.Fpm, ... 'Fph', args.Fph, ... 'Fpr', args.Fpr, ... 'Mpm', args.Mpm, ... 'Mph', args.Mph, ... 'Mpr', args.Mpr); stewart = initializeCylindricalStruts(stewart, ... 'Fsm', args.Fsm, ... 'Fsh', args.Fsh, ... 'Fsr', args.Fsr, ... 'Msm', args.Msm, ... 'Msh', args.Msh, ... 'Msr', args.Msr); stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart, ... 'AP', args.AP, ... 'ARB', args.ARB); nano_hexapod = stewart; if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'nano_hexapod', '-append'); else save('mat/nass_model_stages.mat', 'nano_hexapod'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'nano_hexapod', '-append'); else save('matlab/mat/nass_model_stages.mat', 'nano_hexapod'); end end end #+end_src *** =initializeSample=: Sample #+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no function [sample] = initializeSample(args) arguments args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none' args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3 % Height [m] args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m] args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg] end sample = struct(); switch args.type case 'none' sample.type = 0; sample.m = 0; case 'cylindrical' sample.type = 1; sample.H = args.H; sample.R = args.R; sample.m = args.m; end if exist('./mat', 'dir') if exist('./mat/nass_model_stages.mat', 'file') save('mat/nass_model_stages.mat', 'sample', '-append'); else save('mat/nass_model_stages.mat', 'sample'); end elseif exist('./matlab', 'dir') if exist('./matlab/mat/nass_model_stages.mat', 'file') save('matlab/mat/nass_model_stages.mat', 'sample', '-append'); else save('matlab/mat/nass_model_stages.mat', 'sample'); end end end #+end_src ** Initialize Nano-Hexapod *** =initializeStewartPlatform=: Initialize the Stewart Platform structure #+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no function [stewart] = initializeStewartPlatform() % initializeStewartPlatform - Initialize the stewart structure % % Syntax: [stewart] = initializeStewartPlatform(args) % % Outputs: % - stewart - A structure with the following sub-structures: % - platform_F - % - platform_M - % - joints_F - % - joints_M - % - struts_F - % - struts_M - % - actuators - % - geometry - % - properties - stewart = struct(); stewart.platform_F = struct(); stewart.platform_M = struct(); stewart.joints_F = struct(); stewart.joints_M = struct(); stewart.struts_F = struct(); stewart.struts_M = struct(); stewart.actuators = struct(); stewart.sensors = struct(); stewart.sensors.inertial = struct(); stewart.sensors.force = struct(); stewart.sensors.relative = struct(); stewart.geometry = struct(); stewart.kinematics = struct(); end #+end_src *** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M} #+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no function [stewart] = initializeFramesPositions(stewart, args) % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} % % Syntax: [stewart] = initializeFramesPositions(stewart, 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: % - geometry.H [1x1] - Total Height of the Stewart Platform [m] % - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m] % - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m] % - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m] arguments stewart args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 args.MO_B (1,1) double {mustBeNumeric} = 50e-3 end H = args.H; % Total Height of the Stewart Platform [m] FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m] MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m] stewart.geometry.H = H; stewart.geometry.FO_M = FO_M; stewart.platform_M.MO_B = MO_B; stewart.platform_F.FO_A = FO_A; end #+end_src *** =generateGeneralConfiguration=: Generate a Very General Configuration #+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no 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: % - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} % - platform_M.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} = 115e-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); end Fa = zeros(3,6); Mb = zeros(3,6); for i = 1:6 Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; end stewart.platform_F.Fa = Fa; stewart.platform_M.Mb = Mb; end #+end_src *** =computeJointsPose=: Compute the Pose of the Joints #+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no function [stewart] = computeJointsPose(stewart) % computeJointsPose - % % Syntax: [stewart] = computeJointsPose(stewart) % % Inputs: % - stewart - A structure with the following fields % - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} % - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} % - platform_F.FO_A [3x1] - Position of {A} with respect to {F} % - platform_M.MO_B [3x1] - Position of {B} with respect to {M} % - geometry.FO_M [3x1] - Position of {M} with respect to {F} % % Outputs: % - stewart - A structure with the following added fields % - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A} % - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A} % - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B} % - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B} % - geometry.l [6x1] - The i'th element is the initial length of strut i % - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A} % - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} % - struts_F.l [6x1] - Length of the Fixed part of the i'th strut % - struts_M.l [6x1] - Length of the Mobile part of the i'th strut % - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} % - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') Fa = stewart.platform_F.Fa; assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') Mb = stewart.platform_M.Mb; assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') FO_A = stewart.platform_F.FO_A; assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') MO_B = stewart.platform_M.MO_B; assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M') FO_M = stewart.geometry.FO_M; Aa = Fa - repmat(FO_A, [1, 6]); Bb = Mb - repmat(MO_B, [1, 6]); Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As l = vecnorm(Ab - Aa)'; Bs = (Bb - Ba)./vecnorm(Bb - Ba); FRa = zeros(3,3,6); MRb = zeros(3,3,6); for i = 1:6 FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)]; FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i)); MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); end stewart.geometry.Aa = Aa; stewart.geometry.Ab = Ab; stewart.geometry.Ba = Ba; stewart.geometry.Bb = Bb; stewart.geometry.As = As; stewart.geometry.Bs = Bs; stewart.geometry.l = l; stewart.struts_F.l = l/2; stewart.struts_M.l = l/2; stewart.platform_F.FRa = FRa; stewart.platform_M.MRb = MRb; end #+end_src *** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms #+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no 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: % - platform_F [struct] - structure with the following fields: % - type = 1 % - M [1x1] - Fixed Platform Mass [kg] % - I [3x3] - Fixed Platform Inertia matrix [kg*m^2] % - H [1x1] - Fixed Platform Height [m] % - R [1x1] - Fixed Platform Radius [m] % - platform_M [struct] - structure with the following fields: % - M [1x1] - Mobile Platform Mass [kg] % - I [3x3] - Mobile Platform Inertia matrix [kg*m^2] % - H [1x1] - Mobile Platform Height [m] % - R [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 I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... 1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... 1/2 *args.Fpm * args.Fpr^2]); I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... 1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... 1/2 *args.Mpm * args.Mpr^2]); stewart.platform_F.type = 1; stewart.platform_F.I = I_F; stewart.platform_F.M = args.Fpm; stewart.platform_F.R = args.Fpr; stewart.platform_F.H = args.Fph; stewart.platform_M.type = 1; stewart.platform_M.I = I_M; stewart.platform_M.M = args.Mpm; stewart.platform_M.R = args.Mpr; stewart.platform_M.H = args.Mph; end #+end_src *** =initializeCylindricalStruts=: Define the inertia of cylindrical struts #+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no 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_F [struct] - structure with the following fields: % - M [6x1] - Mass of the Fixed part of the struts [kg] % - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] % - H [6x1] - Height of cylinder for the Fixed part of the struts [m] % - R [6x1] - Radius of cylinder for the Fixed part of the struts [m] % - struts_M [struct] - structure with the following fields: % - M [6x1] - Mass of the Mobile part of the struts [kg] % - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] % - H [6x1] - Height of cylinder for the Mobile part of the struts [m] % - R [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 stewart.struts_M.type = 1; %% Compute the properties of the cylindrical struts Fsm = args.Fsm; Fsh = args.Fsh; Fsr = args.Fsr; Msm = args.Msm; Msh = args.Msh; Msr = args.Msr; I_F = [1/12 * Fsm * (3*Fsr^2 + Fsh^2), ... 1/12 * Fsm * (3*Fsr^2 + Fsh^2), ... 1/2 * Fsm * Fsr^2]; I_M = [1/12 * Msm * (3*Msr^2 + Msh^2), ... 1/12 * Msm * (3*Msr^2 + Msh^2), ... 1/2 * Msm * Msr^2]; stewart.struts_M.I = I_M; stewart.struts_F.I = I_F; stewart.struts_M.M = args.Msm; stewart.struts_M.R = args.Msr; stewart.struts_M.H = args.Msh; stewart.struts_F.type = 1; stewart.struts_F.M = args.Fsm; stewart.struts_F.R = args.Fsr; stewart.struts_F.H = args.Fsh; end #+end_src *** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut #+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no 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: % - K [6x1] - Stiffness of each strut [N/m] % - C [6x1] - Damping of each strut [N/(m/s)] % % Outputs: % - stewart - updated Stewart structure with the added fields: % - actuators.type = 1 % - actuators.K [6x1] - Stiffness of each strut [N/m] % - actuators.C [6x1] - Damping of each strut [N/(m/s)] arguments stewart args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof' args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6 args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6 args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6 args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1 args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6 args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10 args.F_gain (1,1) double {mustBeNumeric} = 1 args.me (1,1) double {mustBeNumeric} = 0.01 args.ma (1,1) double {mustBeNumeric} = 0.01 end if strcmp(args.type, '1dof') stewart.actuators.type = 1; elseif strcmp(args.type, '2dof') stewart.actuators.type = 2; elseif strcmp(args.type, 'flexible') stewart.actuators.type = 3; end stewart.actuators.k = args.k; stewart.actuators.c = args.c; % Parallel stiffness stewart.actuators.kp = args.kp; stewart.actuators.cp = args.cp; stewart.actuators.ka = args.ka; stewart.actuators.ca = args.ca; stewart.actuators.ke = args.ke; stewart.actuators.ce = args.ce; stewart.actuators.F_gain = args.F_gain; stewart.actuators.ma = args.ma; stewart.actuators.me = args.me; end #+end_src *** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints #+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no function [stewart] = initializeJointDynamics(stewart, args) % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints % % Syntax: [stewart] = initializeJointDynamics(args) % % Inputs: % - args - Structure with the following fields: % - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p' % - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p' % - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad] % - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad] % - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)] % - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)] % - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad] % - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad] % - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)] % - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)] % % Outputs: % - stewart - updated Stewart structure with the added fields: % - stewart.joints_F and stewart.joints_M: % - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect) % - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m] % - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad] % - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad] % - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)] % - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)] % - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)] arguments stewart args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof' args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof' args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 args.K_M double {mustBeNumeric} = zeros(6,6) args.M_M double {mustBeNumeric} = zeros(6,6) args.n_xyz_M double {mustBeNumeric} = zeros(2,3) args.xi_M double {mustBeNumeric} = 0.1 args.step_file_M char {} = '' args.K_F double {mustBeNumeric} = zeros(6,6) args.M_F double {mustBeNumeric} = zeros(6,6) args.n_xyz_F double {mustBeNumeric} = zeros(2,3) args.xi_F double {mustBeNumeric} = 0.1 args.step_file_F char {} = '' end switch args.type_F case '2dof' stewart.joints_F.type = 1; case '3dof' stewart.joints_F.type = 2; case '4dof' stewart.joints_F.type = 3; case '6dof' stewart.joints_F.type = 4; case 'flexible' stewart.joints_F.type = 5; otherwise error("joints_F are not correctly defined") end switch args.type_M case '2dof' stewart.joints_M.type = 1; case '3dof' stewart.joints_M.type = 2; case '4dof' stewart.joints_M.type = 3; case '6dof' stewart.joints_M.type = 4; case 'flexible' stewart.joints_M.type = 5; otherwise error("joints_M are not correctly defined") end stewart.joints_M.Ka = args.Ka_M; stewart.joints_M.Kr = args.Kr_M; stewart.joints_F.Ka = args.Ka_F; stewart.joints_F.Kr = args.Kr_F; stewart.joints_M.Ca = args.Ca_M; stewart.joints_M.Cr = args.Cr_M; stewart.joints_F.Ca = args.Ca_F; stewart.joints_F.Cr = args.Cr_F; stewart.joints_M.Kf = args.Kf_M; stewart.joints_M.Kt = args.Kt_M; stewart.joints_F.Kf = args.Kf_F; stewart.joints_F.Kt = args.Kt_F; stewart.joints_M.Cf = args.Cf_M; stewart.joints_M.Ct = args.Ct_M; stewart.joints_F.Cf = args.Cf_F; stewart.joints_F.Ct = args.Ct_F; stewart.joints_F.M = args.M_F; stewart.joints_F.K = args.K_F; stewart.joints_F.n_xyz = args.n_xyz_F; stewart.joints_F.xi = args.xi_F; stewart.joints_F.xi = args.xi_F; stewart.joints_F.step_file = args.step_file_F; stewart.joints_M.M = args.M_M; stewart.joints_M.K = args.K_M; stewart.joints_M.n_xyz = args.n_xyz_M; stewart.joints_M.xi = args.xi_M; stewart.joints_M.step_file = args.step_file_M; end #+end_src *** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose #+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no function [stewart] = initializeStewartPose(stewart, args) % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose % It uses the inverse kinematic % % Syntax: [stewart] = initializeStewartPose(stewart, args) % % 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: % - stewart - updated Stewart structure with the added fields: % - actuators.Leq [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, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); stewart.actuators.Leq = dLi; end #+end_src *** =computeJacobian=: Compute the Jacobian Matrix #+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no function [stewart] = computeJacobian(stewart) % computeJacobian - % % Syntax: [stewart] = computeJacobian(stewart) % % Inputs: % - stewart - With at least the following fields: % - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A} % - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A} % - actuators.K [6x1] - Total stiffness of the actuators % % Outputs: % - stewart - With the 3 added field: % - geometry.J [6x6] - The Jacobian Matrix % - geometry.K [6x6] - The Stiffness Matrix % - geometry.C [6x6] - The Compliance Matrix assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As') As = stewart.geometry.As; assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab') Ab = stewart.geometry.Ab; assert(isfield(stewart.actuators, 'k'), 'stewart.actuators should have attribute k') Ki = stewart.actuators.k; J = [As' , cross(Ab, As)']; K = J'*diag(Ki)*J; C = inv(K); stewart.geometry.J = J; stewart.geometry.K = K; stewart.geometry.C = C; end #+end_src *** =inverseKinematics=: Compute Inverse Kinematics #+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no 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 % - geometry.Aa [3x6] - The positions ai expressed in {A} % - geometry.Bb [3x6] - The positions bi expressed in {B} % - geometry.l [6x1] - Length of each strut % - 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 assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa') Aa = stewart.geometry.Aa; assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb') Bb = stewart.geometry.Bb; assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l') l = stewart.geometry.l; Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa)); dLi = Li-l; end #+end_src *** =displayArchitecture=: 3D plot of the Stewart platform architecture :PROPERTIES: :header-args:matlab+: :tangle matlab/src/displayArchitecture.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/displayArchitecture.m][here]]. **** Function description :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab function [] = displayArchitecture(stewart, args) % displayArchitecture - 3D plot of the Stewart platform architecture % % Syntax: [] = displayArchitecture(args) % % Inputs: % - stewart % - args - Structure with 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} % - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} % - F_color [color] - Color used for the Fixed elements % - M_color [color] - Color used for the Mobile elements % - L_color [color] - Color used for the Legs elements % - frames [true/false] - Display the Frames % - legs [true/false] - Display the Legs % - joints [true/false] - Display the Joints % - labels [true/false] - Display the Labels % - platforms [true/false] - Display the Platforms % - views ['all', 'xy', 'yz', 'xz', 'default'] - % % Outputs: #+end_src **** Optional Parameters :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab arguments stewart args.AP (3,1) double {mustBeNumeric} = zeros(3,1) args.ARB (3,3) double {mustBeNumeric} = eye(3) args.F_color = [0 0.4470 0.7410] args.M_color = [0.8500 0.3250 0.0980] args.L_color = [0 0 0] args.frames logical {mustBeNumericOrLogical} = true args.legs logical {mustBeNumericOrLogical} = true args.joints logical {mustBeNumericOrLogical} = true args.labels logical {mustBeNumericOrLogical} = true args.platforms logical {mustBeNumericOrLogical} = true args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default' end #+end_src **** Check the =stewart= structure elements :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') FO_A = stewart.platform_F.FO_A; assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') MO_B = stewart.platform_M.MO_B; assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') H = stewart.geometry.H; assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') Fa = stewart.platform_F.Fa; assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') Mb = stewart.platform_M.Mb; #+end_src **** Figure Creation, Frames and Homogeneous transformations :PROPERTIES: :UNNUMBERED: t :END: The reference frame of the 3d plot corresponds to the frame $\{F\}$. #+begin_src matlab if ~strcmp(args.views, 'all') figure; else f = figure('visible', 'off'); end hold on; #+end_src We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$. #+begin_src matlab FTa = [eye(3), FO_A; ... zeros(1,3), 1]; ATb = [args.ARB, args.AP; ... zeros(1,3), 1]; BTm = [eye(3), -MO_B; ... zeros(1,3), 1]; FTm = FTa*ATb*BTm; #+end_src Let's define a parameter that define the length of the unit vectors used to display the frames. #+begin_src matlab d_unit_vector = H/4; #+end_src Let's define a parameter used to position the labels with respect to the center of the element. #+begin_src matlab d_label = H/20; #+end_src **** Fixed Base elements :PROPERTIES: :UNNUMBERED: t :END: Let's first plot the frame $\{F\}$. #+begin_src matlab Ff = [0, 0, 0]; if args.frames quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ... [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) if args.labels text(Ff(1) + d_label, ... Ff(2) + d_label, ... Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color); end end #+end_src Now plot the frame $\{A\}$ fixed to the Base. #+begin_src matlab if args.frames quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ... [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color) if args.labels text(FO_A(1) + d_label, ... FO_A(2) + d_label, ... FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color); end end #+end_src Let's then plot the circle corresponding to the shape of the Fixed base. #+begin_src matlab if args.platforms && stewart.platform_F.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal center = [0; 0; 0]; % Center of the circle radius = stewart.platform_F.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); plot3(points(1,:), ... points(2,:), ... points(3,:), '-', 'Color', args.F_color); end #+end_src Let's now plot the position and labels of the Fixed Joints #+begin_src matlab if args.joints scatter3(Fa(1,:), ... Fa(2,:), ... Fa(3,:), 'MarkerEdgeColor', args.F_color); if args.labels for i = 1:size(Fa,2) text(Fa(1,i) + d_label, ... Fa(2,i), ... Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color); end end end #+end_src **** Mobile Platform elements :PROPERTIES: :UNNUMBERED: t :END: Plot the frame $\{M\}$. #+begin_src matlab Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F} if args.frames FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ... FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color) if args.labels text(Fm(1) + d_label, ... Fm(2) + d_label, ... Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color); end end #+end_src Plot the frame $\{B\}$. #+begin_src matlab FB = FO_A + args.AP; if args.frames FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ... FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color) if args.labels text(FB(1) - d_label, ... FB(2) + d_label, ... FB(3) + d_label, '$\{B\}$', 'Color', args.M_color); end end #+end_src Let's then plot the circle corresponding to the shape of the Mobile platform. #+begin_src matlab if args.platforms && stewart.platform_M.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal center = Fm(1:3); % Center of the circle radius = stewart.platform_M.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); plot3(points(1,:), ... points(2,:), ... points(3,:), '-', 'Color', args.M_color); end #+end_src Plot the position and labels of the rotation joints fixed to the mobile platform. #+begin_src matlab if args.joints Fb = FTm*[Mb;ones(1,6)]; scatter3(Fb(1,:), ... Fb(2,:), ... Fb(3,:), 'MarkerEdgeColor', args.M_color); if args.labels for i = 1:size(Fb,2) text(Fb(1,i) + d_label, ... Fb(2,i), ... Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color); end end end #+end_src **** Legs :PROPERTIES: :UNNUMBERED: t :END: Plot the legs connecting the joints of the fixed base to the joints of the mobile platform. #+begin_src matlab if args.legs for i = 1:6 plot3([Fa(1,i), Fb(1,i)], ... [Fa(2,i), Fb(2,i)], ... [Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color); if args.labels text((Fa(1,i)+Fb(1,i))/2 + d_label, ... (Fa(2,i)+Fb(2,i))/2, ... (Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color); end end end #+end_src **** Figure parameters #+begin_src matlab switch args.views case 'default' view([1 -0.6 0.4]); case 'xy' view([0 0 1]); case 'xz' view([0 -1 0]); case 'yz' view([1 0 0]); end axis equal; axis off; #+end_src **** Subplots #+begin_src matlab if strcmp(args.views, 'all') hAx = findobj('type', 'axes'); figure; s1 = subplot(2,2,1); copyobj(get(hAx(1), 'Children'), s1); view([0 0 1]); axis equal; axis off; title('Top') s2 = subplot(2,2,2); copyobj(get(hAx(1), 'Children'), s2); view([1 -0.6 0.4]); axis equal; axis off; s3 = subplot(2,2,3); copyobj(get(hAx(1), 'Children'), s3); view([1 0 0]); axis equal; axis off; title('Front') s4 = subplot(2,2,4); copyobj(get(hAx(1), 'Children'), s4); view([0 -1 0]); axis equal; axis off; title('Side') close(f); end #+end_src *** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform :PROPERTIES: :header-args:matlab+: :tangle matlab/src/describeStewartPlatform.m :header-args:matlab+: :comments none :mkdirp yes :eval no :END: <> This Matlab function is accessible [[file:../src/describeStewartPlatform.m][here]]. **** Function description :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab function [] = describeStewartPlatform(stewart) % describeStewartPlatform - Display some text describing the current defined Stewart Platform % % Syntax: [] = describeStewartPlatform(args) % % Inputs: % - stewart % % Outputs: #+end_src **** Optional Parameters :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab arguments stewart end #+end_src **** Geometry #+begin_src matlab fprintf('GEOMETRY:\n') fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H) if stewart.platform_M.MO_B(3) > 0 fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3)) else fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3)) end fprintf('- The initial length of the struts are:\n') fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l) fprintf('\n') #+end_src **** Actuators #+begin_src matlab fprintf('ACTUATORS:\n') if stewart.actuators.type == 1 fprintf('- The actuators are modelled as 1DoF.\n') fprintf('- The Stiffness and Damping of each actuators is:\n') fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k(1), stewart.actuators.c(1)) if stewart.actuators.kp > 0 fprintf('\t Added parallel stiffness: kp = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.kp(1)) end elseif stewart.actuators.type == 2 fprintf('- The actuators are modelled as 2DoF (APA).\n') fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n') fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.ka(1), stewart.actuators.ca(1)) fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n') fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.kr(1), stewart.actuators.cr(1)) elseif stewart.actuators.type == 3 fprintf('- The actuators are modelled with a flexible element (FEM).\n') end fprintf('\n') #+end_src **** Joints #+begin_src matlab fprintf('JOINTS:\n') #+end_src Type of the joints on the fixed base. #+begin_src matlab switch stewart.joints_F.type case 1 fprintf('- The joints on the fixed based are universal joints (2DoF)\n') case 2 fprintf('- The joints on the fixed based are spherical joints (3DoF)\n') end #+end_src Type of the joints on the mobile platform. #+begin_src matlab switch stewart.joints_M.type case 1 fprintf('- The joints on the mobile based are universal joints (2DoF)\n') case 2 fprintf('- The joints on the mobile based are spherical joints (3DoF)\n') end #+end_src Position of the fixed joints #+begin_src matlab fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n') fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa) #+end_src Position of the mobile joints #+begin_src matlab fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n') fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb) fprintf('\n') #+end_src **** Kinematics #+begin_src matlab fprintf('KINEMATICS:\n') if isfield(stewart.kinematics, 'K') fprintf('- The Stiffness matrix K is (in [N/m]):\n') fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K) end if isfield(stewart.kinematics, 'C') fprintf('- The Damping matrix C is (in [m/N]):\n') fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C) end #+end_src * Helping Functions :noexport: ** Initialize Path #+NAME: m-init-path #+BEGIN_SRC matlab addpath('./matlab/'); % Path for scripts %% Path for functions, data and scripts addpath('./matlab/mat/'); % Path for Computed FRF addpath('./matlab/src/'); % Path for functions addpath('./matlab/STEPS/'); % Path for STEPS addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files %% Data directory data_dir = './matlab/mat/' #+END_SRC #+NAME: m-init-path-tangle #+BEGIN_SRC matlab %% Path for functions, data and scripts addpath('./mat/'); % Path for Data addpath('./src/'); % Path for functions addpath('./STEPS/'); % Path for STEPS addpath('./subsystems/'); % Path for Subsystems Simulink files %% Data directory data_dir = './mat/'; #+END_SRC ** Initialize Simscape Model #+NAME: m-init-simscape #+begin_src matlab % Simulink Model name mdl = 'nass_model'; #+end_src ** Initialize other elements #+NAME: m-init-other #+BEGIN_SRC matlab %% Colors for the figures colors = colororder; #+END_SRC