commit c72817bdcc1e298feaf15b10d1b99970b571f573 Author: Thomas Dehaeze Date: Wed Nov 26 10:30:18 2025 +0100 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0e3ab6a --- /dev/null +++ b/.gitignore @@ -0,0 +1,259 @@ +ltximg/ +*.autosave +slprj/ +matlab/slprj/ +*.slxc + + +# ============================================================ +# ============================================================ +# LATEX +# ============================================================ +# ============================================================ + +## Core latex/pdflatex auxiliary files: +*.aux +*.lof +*.log +*.lot +*.fls +*.out +*.toc +*.fmt +*.fot +*.cb +*.cb2 +.*.lb + +## Intermediate documents: +*.dvi +*.xdv +*-converted-to.* +# these rules might exclude image files for figures etc. +# *.ps +# *.eps +# *.pdf + +## Generated if empty string is given at "Please type another file name for output:" +.pdf + +## Bibliography auxiliary files (bibtex/biblatex/biber): +*.bbl +*.bcf +*.blg +*-blx.aux +*-blx.bib +*.run.xml + +## Build tool auxiliary files: +*.fdb_latexmk +*.synctex +*.synctex(busy) +*.synctex.gz +*.synctex.gz(busy) +*.pdfsync + +## Build tool directories for auxiliary files +# latexrun +latex.out/ + +## Auxiliary and intermediate files from other packages: +# algorithms +*.alg +*.loa + +# achemso +acs-*.bib + +# amsthm +*.thm + +# beamer +*.nav +*.pre +*.snm +*.vrb + +# changes +*.soc + +# cprotect +*.cpt + +# elsarticle (documentclass of Elsevier journals) +*.spl + +# endnotes +*.ent + +# fixme +*.lox + +# feynmf/feynmp +*.mf +*.mp +*.t[1-9] +*.t[1-9][0-9] +*.tfm + +#(r)(e)ledmac/(r)(e)ledpar +*.end +*.?end +*.[1-9] +*.[1-9][0-9] +*.[1-9][0-9][0-9] +*.[1-9]R +*.[1-9][0-9]R +*.[1-9][0-9][0-9]R +*.eledsec[1-9] +*.eledsec[1-9]R +*.eledsec[1-9][0-9] +*.eledsec[1-9][0-9]R +*.eledsec[1-9][0-9][0-9] +*.eledsec[1-9][0-9][0-9]R + +# glossaries +*.acn +*.acr +*.glg +*.glo +*.gls +*.glsdefs + +# gnuplottex +*-gnuplottex-* + +# gregoriotex +*.gaux +*.gtex + +# htlatex +*.4ct +*.4tc +*.idv +*.lg +*.trc +*.xref + +# hyperref +*.brf + +# knitr +*-concordance.tex +# TODO Comment the next line if you want to keep your tikz graphics files +*.tikz +*-tikzDictionary + +# listings +*.lol + +# makeidx +*.idx +*.ilg +*.ind +*.ist + +# minitoc +*.maf +*.mlf +*.mlt +*.mtc[0-9]* +*.slf[0-9]* +*.slt[0-9]* +*.stc[0-9]* + +# minted +_minted* +*.pyg + +# morewrites +*.mw + +# nomencl +*.nlg +*.nlo +*.nls + +# pax +*.pax + +# pdfpcnotes +*.pdfpc + +# sagetex +*.sagetex.sage +*.sagetex.py +*.sagetex.scmd + +# scrwfile +*.wrt + +# sympy +*.sout +*.sympy +sympy-plots-for-*.tex/ + +# pdfcomment +*.upa +*.upb + +# pythontex +*.pytxcode +pythontex-files-*/ + +# thmtools +*.loe + +# TikZ & PGF +*.dpth +*.md5 +*.auxlock + +# todonotes +*.tdo + +# easy-todo +*.lod + +# xmpincl +*.xmpi + +# xindy +*.xdy + +# xypic precompiled matrices +*.xyc + +# endfloat +*.ttt +*.fff + +# Latexian +TSWLatexianTemp* + +## Editors: +# WinEdt +*.bak +*.sav + +# Texpad +.texpadtmp + +# LyX +*.lyx~ + +# Kile +*.backup + +# KBibTeX +*~[0-9]* + +# auto folder when using emacs and auctex +./auto/* +*.el + +# expex forward references with \gathertags +*-tags.tex + +# standalone packages +*.sta diff --git a/inkscape/figs b/inkscape/figs new file mode 120000 index 0000000..30fa883 --- /dev/null +++ b/inkscape/figs @@ -0,0 +1 @@ +../paper/figs \ No newline at end of file diff --git a/inkscape/tikz.org b/inkscape/tikz.org new file mode 100644 index 0000000..2ab28a1 --- /dev/null +++ b/inkscape/tikz.org @@ -0,0 +1,216 @@ +#+TITLE: Decoupling Properties of the Cubic Architecture +:DRAWER: +#+LANGUAGE: en +#+EMAIL: dehaeze.thomas@gmail.com +#+AUTHOR: Dehaeze Thomas + +#+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 + +#+begin_src latex :file detail_kinematics_cubic_schematic_full.pdf :results file + \begin{tikzpicture} + \begin{scope}[rotate={45}, shift={(0, 0, -4)}] + % We first define the coordinate of the points of the Cube + \coordinate[] (bot) at (0,0,4); + \coordinate[] (top) at (4,4,0); + \coordinate[] (A1) at (0,0,0); + \coordinate[] (A2) at (4,0,4); + \coordinate[] (A3) at (0,4,4); + \coordinate[] (B1) at (4,0,0); + \coordinate[] (B2) at (4,4,4); + \coordinate[] (B3) at (0,4,0); + + % Center of the Cube + \coordinate[] (cubecenter) at ($0.5*(bot) + 0.5*(top)$); + + % We draw parts of the cube that corresponds to the Stewart platform + \draw[] (A1)node[]{$\bullet$} -- (B1)node[]{$\bullet$} -- (A2)node[]{$\bullet$} -- (B2)node[]{$\bullet$} -- (A3)node[]{$\bullet$} -- (B3)node[]{$\bullet$} -- (A1); + + % ai and bi are computed + \def\lfrom{0.0} + \def\lto{1.0} + + \coordinate(a1) at ($(A1) - \lfrom*(A1) + \lfrom*(B1)$); + \coordinate(b1) at ($(A1) - \lto*(A1) + \lto*(B1)$); + \coordinate(a2) at ($(A2) - \lfrom*(A2) + \lfrom*(B1)$); + \coordinate(b2) at ($(A2) - \lto*(A2) + \lto*(B1)$); + \coordinate(a3) at ($(A2) - \lfrom*(A2) + \lfrom*(B2)$); + \coordinate(b3) at ($(A2) - \lto*(A2) + \lto*(B2)$); + \coordinate(a4) at ($(A3) - \lfrom*(A3) + \lfrom*(B2)$); + \coordinate(b4) at ($(A3) - \lto*(A3) + \lto*(B2)$); + \coordinate(a5) at ($(A3) - \lfrom*(A3) + \lfrom*(B3)$); + \coordinate(b5) at ($(A3) - \lto*(A3) + \lto*(B3)$); + \coordinate(a6) at ($(A1) - \lfrom*(A1) + \lfrom*(B3)$); + \coordinate(b6) at ($(A1) - \lto*(A1) + \lto*(B3)$); + + % We draw the fixed and mobiles platforms + \path[fill=colorblue, opacity=0.2] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle; + \path[fill=colorblue, opacity=0.2] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle; + \draw[color=colorblue, dashed] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle; + \draw[color=colorblue, dashed] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle; + + % The legs of the hexapod are drawn + \draw[color=colorblue] (a1)node{$\bullet$} -- (b1)node{$\bullet$}; + \draw[color=colorblue] (a2)node{$\bullet$} -- (b2)node{$\bullet$}; + \draw[color=colorblue] (a3)node{$\bullet$} -- (b3)node{$\bullet$}; + \draw[color=colorblue] (a4)node{$\bullet$} -- (b4)node{$\bullet$}; + \draw[color=colorblue] (a5)node{$\bullet$} -- (b5)node{$\bullet$}; + \draw[color=colorblue] (a6)node{$\bullet$} -- (b6)node{$\bullet$}; + + % Unit vector + \draw[color=colorred, ->] ($0.9*(a1)+0.1*(b1)$)node{$\bullet$} -- ($0.65*(a1)+0.35*(b1)$)node[right]{$\hat{\bm{s}}_3$}; + \draw[color=colorred, ->] ($0.9*(a2)+0.1*(b2)$)node{$\bullet$} -- ($0.65*(a2)+0.35*(b2)$)node[left]{$\hat{\bm{s}}_4$}; + \draw[color=colorred, ->] ($0.9*(a3)+0.1*(b3)$)node{$\bullet$} -- ($0.65*(a3)+0.35*(b3)$)node[below]{$\hat{\bm{s}}_5$}; + \draw[color=colorred, ->] ($0.9*(a4)+0.1*(b4)$)node{$\bullet$} -- ($0.65*(a4)+0.35*(b4)$)node[below]{$\hat{\bm{s}}_6$}; + \draw[color=colorred, ->] ($0.9*(a5)+0.1*(b5)$)node{$\bullet$} -- ($0.65*(a5)+0.35*(b5)$)node[left]{$\hat{\bm{s}}_1$}; + \draw[color=colorred, ->] ($0.9*(a6)+0.1*(b6)$)node{$\bullet$} -- ($0.65*(a6)+0.35*(b6)$)node[right]{$\hat{\bm{s}}_2$}; + + % Labels + \node[above=0.1 of B1] {$\tilde{\bm{b}}_3 = \tilde{\bm{b}}_4$}; + \node[above=0.1 of B2] {$\tilde{\bm{b}}_5 = \tilde{\bm{b}}_6$}; + \node[above=0.1 of B3] {$\tilde{\bm{b}}_1 = \tilde{\bm{b}}_2$}; + \end{scope} + + % Height of the Hexapod + \coordinate[] (sizepos) at ($(a2)+(0.2, 0)$); + \coordinate[] (origin) at (0,0,0); + + \draw[->, color=colorgreen] (cubecenter.center) node[above right]{$\{B\}$} -- ++(0,0,1); + \draw[->, color=colorgreen] (cubecenter.center) -- ++(1,0,0); + \draw[->, color=colorgreen] (cubecenter.center) -- ++(0,1,0); + + \node[] at (cubecenter.center){$\bullet$}; + \node[above left] at (cubecenter.center){$\{C\}$}; + + % Useful part of the cube + \draw[<->, dashed] ($(A2)+(0.5,0)$) -- node[midway, right]{$H_{C}$} ($(B1)+(0.5,0)$); + \end{tikzpicture} +#+end_src + +#+RESULTS: +[[file:figs/detail_kinematics_cubic_schematic_full.png]] + +#+begin_src latex :file detail_kinematics_cubic_schematic.pdf :results file + \begin{tikzpicture} + \begin{scope}[rotate={45}, shift={(0, 0, -4)}] + % We first define the coordinate of the points of the Cube + \coordinate[] (bot) at (0,0,4); + \coordinate[] (top) at (4,4,0); + \coordinate[] (A1) at (0,0,0); + \coordinate[] (A2) at (4,0,4); + \coordinate[] (A3) at (0,4,4); + \coordinate[] (B1) at (4,0,0); + \coordinate[] (B2) at (4,4,4); + \coordinate[] (B3) at (0,4,0); + + % Center of the Cube + \coordinate[] (cubecenter) at ($0.5*(bot) + 0.5*(top)$); + + % We draw parts of the cube that corresponds to the Stewart platform + \draw[] (A1)node[]{$\bullet$} -- (B1)node[]{$\bullet$} -- (A2)node[]{$\bullet$} -- (B2)node[]{$\bullet$} -- (A3)node[]{$\bullet$} -- (B3)node[]{$\bullet$} -- (A1); + + % ai and bi are computed + \def\lfrom{0.2} + \def\lto{0.8} + + \coordinate(a1) at ($(A1) - \lfrom*(A1) + \lfrom*(B1)$); + \coordinate(b1) at ($(A1) - \lto*(A1) + \lto*(B1)$); + \coordinate(a2) at ($(A2) - \lfrom*(A2) + \lfrom*(B1)$); + \coordinate(b2) at ($(A2) - \lto*(A2) + \lto*(B1)$); + \coordinate(a3) at ($(A2) - \lfrom*(A2) + \lfrom*(B2)$); + \coordinate(b3) at ($(A2) - \lto*(A2) + \lto*(B2)$); + \coordinate(a4) at ($(A3) - \lfrom*(A3) + \lfrom*(B2)$); + \coordinate(b4) at ($(A3) - \lto*(A3) + \lto*(B2)$); + \coordinate(a5) at ($(A3) - \lfrom*(A3) + \lfrom*(B3)$); + \coordinate(b5) at ($(A3) - \lto*(A3) + \lto*(B3)$); + \coordinate(a6) at ($(A1) - \lfrom*(A1) + \lfrom*(B3)$); + \coordinate(b6) at ($(A1) - \lto*(A1) + \lto*(B3)$); + + % We draw the fixed and mobiles platforms + \path[fill=colorblue, opacity=0.2] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle; + \path[fill=colorblue, opacity=0.2] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle; + \draw[color=colorblue, dashed] (a1) -- (a2) -- (a3) -- (a4) -- (a5) -- (a6) -- cycle; + \draw[color=colorblue, dashed] (b1) -- (b2) -- (b3) -- (b4) -- (b5) -- (b6) -- cycle; + + % The legs of the hexapod are drawn + \draw[color=colorblue] (a1)node{$\bullet$} -- (b1)node{$\bullet$}node[below right]{$\bm{b}_3$}; + \draw[color=colorblue] (a2)node{$\bullet$} -- (b2)node{$\bullet$}node[right]{$\bm{b}_4$}; + \draw[color=colorblue] (a3)node{$\bullet$} -- (b3)node{$\bullet$}node[above right]{$\bm{b}_5$}; + \draw[color=colorblue] (a4)node{$\bullet$} -- (b4)node{$\bullet$}node[above left]{$\bm{b}_6$}; + \draw[color=colorblue] (a5)node{$\bullet$} -- (b5)node{$\bullet$}node[left]{$\bm{b}_1$}; + \draw[color=colorblue] (a6)node{$\bullet$} -- (b6)node{$\bullet$}node[below left]{$\bm{b}_2$}; + \end{scope} + + % Height of the Hexapod + \coordinate[] (sizepos) at ($(a2)+(0.2, 0)$); + \coordinate[] (origin) at (0,0,0); + + \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) node[above right]{$\{B\}$} -- ++(0,0,1); + \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) -- ++(1,0,0); + \draw[->, color=colorgreen] ($(cubecenter.center)+(0,2.0,0)$) -- ++(0,1,0); + + \node[] at (cubecenter.center){$\bullet$}; + \node[right] at (cubecenter.center){$\{C\}$}; + + \draw[<->, dashed] (cubecenter.center) -- node[midway, right]{$H$} ($(cubecenter.center)+(0,2.0,0)$); + \end{tikzpicture} +#+end_src + +#+RESULTS: +[[file:figs/detail_kinematics_cubic_schematic.png]] + +#+begin_src latex :file detail_kinematics_centralized_control.pdf +\begin{tikzpicture} + \node[block] (Jt) at (0, 0) {$\bm{J}^{-\intercal}$}; + \node[block, right= of Jt] (G) {$\bm{G}$}; + \node[block, right= of G] (J) {$\bm{J}^{-1}$}; + \node[block, left= of Jt] (Kx) {$\bm{K}_{\mathcal{X}}$}; + + \draw[->] (Kx.east) -- node[midway, above]{$\bm{\mathcal{F}}$} (Jt.west); + \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$}; + \draw[->] (G.east) -- (J.west) node[above left]{$\bm{\mathcal{L}}$}; + \draw[->] (J.east) -- ++(1.0, 0); + \draw[->] ($(J.east) + (0.5, 0)$)node[]{$\bullet$} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -1) -| ($(Kx.west) + (-0.5, 0)$) -- (Kx.west); + + \begin{scope}[on background layer] + \node[fit={(Jt.south west) (J.north east)}, fill=black!20!white, draw, dashed, inner sep=4pt] (Px) {}; + \node[anchor={south}] at (Px.north){\small{Cartesian Plant}}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+RESULTS: +[[file:figs/detail_kinematics_centralized_control.png]] + +#+begin_src latex :file detail_kinematics_decentralized_control.pdf +\begin{tikzpicture} + \node[block] (G) at (0,0) {$\bm{G}$}; + + \node[block, left= of G] (Kl) {$\bm{K}_{\mathcal{L}}$}; + + \draw[->] (Kl.east) -- node[midway, above]{$\bm{\tau}$} (G.west); + \draw[->] (G.east) -- ++(1.0, 0); + \draw[->] ($(G.east) + (0.5, 0)$)node[]{$\bullet$} node[above]{$\bm{\mathcal{L}}$} -- ++(0, -1) -| ($(Kl.west) + (-0.5, 0)$) -- (Kl.west); + + \begin{scope}[on background layer] + \node[fit={(G.south west) (G.north east)}, fill=black!20!white, draw, dashed, inner sep=4pt] (Pl) {}; + \node[anchor={south}] at (Pl.north){\small{Strut Plant}}; + \end{scope} +\end{tikzpicture} +#+end_src + +#+RESULTS: +[[file:figs/detail_kinematics_decentralized_control.png]] diff --git a/matlab/dehaeze26_cubic_architecture.m b/matlab/dehaeze26_cubic_architecture.m new file mode 100644 index 0000000..304bf78 --- /dev/null +++ b/matlab/dehaeze26_cubic_architecture.m @@ -0,0 +1,749 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +%% Path for functions, data and scripts +addpath('./src/'); % Path for functions +addpath('./subsystems/'); % Path for Subsystems Simulink files + +% Simulink Model name +mdl = 'nano_hexapod_model'; + +%% Colors for the figures +colors = colororder; + +%% Example of a typical "cubic" architecture +MO_B = -50e-3; % Position {B} with respect to {M} [m] + +H = 100e-3; % Height of the Stewart platform [m] + +Hc = 100e-3; % Size of the useful part of the cube [m] + +FOc = H + MO_B; % Center of the cube with respect to {F} + % here positionned at the frame {B} + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1); +stewart = initializeJointDynamics(stewart); +stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); +stewart = initializeCylindricalStruts(stewart); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); +view([40, 5]); + +%% Example of a typical "cubic" architecture +MO_B = -20e-3; % Position {B} with respect to {M} [m] + +H = 40e-3; % Height of the Stewart platform [m] + +Hc = 100e-3; % Size of the useful part of the cube [m] + +FOc = H + MO_B; % Center of the cube with respect to {F} + % here positionned at the frame {B} + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1); +stewart = computeJacobian(stewart); +stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); +view([40, 5]); + +%% Analytical formula for Stiffness matrix of the Cubic Stewart platform +% Define symbolic variables +syms k Hc alpha H + +assume(k > 0); % k is positive real +assume(Hc, 'real'); % Hc is real +assume(H, 'real'); % H is real +assume(alpha, 'real'); % alpha is real + +% Define si matrix (edges of the cubes) +si = 1/sqrt(3)*[ + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1]; ... + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1] ... +]; + +% Define ci matrix (vertices of the cubes) +ci = Hc * [ + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [-sqrt(2), 0, 0.5]; ... + [-sqrt(2), 0, 0.5] ... +]; + +% Apply vertical shift to ci +ci = ci + H * [0, 0, 1]; + +% Calculate bi vectors (Stewart platform top joints) +bi = ci + alpha * si; + +% Initialize stiffness matrix +K = sym(zeros(6,6)); + +% Calculate elements of the stiffness matrix +for i = 1:6 + % Extract vectors for each leg + s_i = si(i,:)'; + b_i = bi(i,:)'; + + % Calculate cross product vector + cross_bs = cross(b_i, s_i); + + % Build matrix blocks + K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs'; + K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i'; + K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i'; + K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs'; +end + +% Scale by stiffness coefficient +K = k * K; + +% Simplify the expressions +K = simplify(K); + +% Display the analytical stiffness matrix +disp('Analytical Stiffness Matrix:'); +pretty(K); + +%% Cubic configuration +H = 100e-3; % Height of the Stewart platform [m] +Hc = 100e-3; % Size of the useful part of the cube [m] +FOc = 50e-3; % Center of the cube at the Stewart platform center +MO_B = -50e-3; % Position {B} with respect to {M} [m] +MHb = 0; + +stewart_cubic = initializeStewartPlatform(); +stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B); +stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb); +stewart_cubic = computeJointsPose(stewart_cubic); +stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1); +stewart_cubic = computeJacobian(stewart_cubic); +stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3); + +% Let's now define the actuator stroke. +L_max = 50e-6; % [m] + +%% Mobility of a Stewart platform with Cubic architecture - Translations +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 200); +rs = zeros(length(thetas), length(phis)); + +for i = 1:length(thetas) + for j = 1:length(phis) + Tx = sin(thetas(i))*cos(phis(j)); + Ty = sin(thetas(i))*sin(phis(j)); + Tz = cos(thetas(i)); + + dL = stewart_cubic.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction + + rs(i, j) = L_max/max(abs(dL)); + % rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max])); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); +X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid); +Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid); +Z = 1e6 * rs .* cos(theta_grid); + +figure; +hold on; +surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:)); +quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); + +%% Mobility of a Stewart platform with Cubic architecture - Rotations +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 200); +rs_cubic = zeros(length(thetas), length(phis)); + +for i = 1:length(thetas) + for j = 1:length(phis) + Rx = sin(thetas(i))*cos(phis(j)); + Ry = sin(thetas(i))*sin(phis(j)); + Rz = cos(thetas(i)); + + dL = stewart_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;]; + rs_cubic(i, j) = L_max/max(abs(dL)); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); + +X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid); +Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid); +Z_cubic = 1e6 * rs_cubic .* cos(theta_grid); + +figure; +hold on; +surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:)); +quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); + +%% Input/Output definition of the Simscape model +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad] + +% Prepare simulation +controller = initializeController('type', 'open-loop'); +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); + +%% Cubic Stewart platform with payload above the top platform - B frame at the CoM +H = 200e-3; % height of the Stewart platform [m] +MO_B = 50e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).'; +G_CoM.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +%% Same geometry but B Frame at cube's center (CoK) +MO_B = -100e-3; % Position {B} with respect to {M} [m] +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); +G_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +% for i = 1:5 +% for j = i+1:6 +% plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... +% 'HandleVisibility', 'off'); +% end +% end +plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude'); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) + +%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +% for i = 1:5 +% for j = i+1:6 +% plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... +% 'HandleVisibility', 'off'); +% end +% end +plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude'); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) + +%% Cubic Stewart platform with payload above the top platform +H = 200e-3; +MO_B = -100e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Sample at the Center of the cube +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3); + +% Run the linearization +G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); +G_CoM_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) + +%% Input/Output definition of the Simscape model +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N] + +% Prepare simulation : Payload above the top platform +controller = initializeController('type', 'open-loop'); +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); + +%% Cubic Stewart platform +H = 200e-3; % height of the Stewart platform [m] +MO_B = 50e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_cubic = linearize(mdl, io); +G_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Non-Cubic Stewart platform +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ... + 'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ... + 'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180)); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_non_cubic = linearize(mdl, io); +G_non_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... + 'DisplayName', '$l_i/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'DisplayName', '$l_i/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 1e-4]) + +%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... + 'DisplayName', '$l_i/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'DisplayName', '$l_i/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 1e-4]) + +%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... + 'DisplayName', '$f_{m,i}/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'DisplayName', '$f_{m,i}/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); ylim([1e-4, 1e2]); + +%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... + 'DisplayName', '$f_{m,i}/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'DisplayName', '$f_{m,i}/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); ylim([1e-4, 1e2]); + +%% Cubic configurations with center of the cube above the top platform +H = 100e-3; % height of the Stewart platform [m] +MO_B = 20e-3; % Position {B} with respect to {M} [m] +FOc = H + MO_B; % Center of the cube with respect to {F} + +%% Small cube +Hc = 2*MO_B; % Size of the useful part of the cube [m] + +stewart_small = initializeStewartPlatform(); +stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B); +stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_small = computeJointsPose(stewart_small); +stewart_small = initializeStrutDynamics(stewart_small, 'k', 1); +stewart_small = computeJacobian(stewart_small); +stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb))); + +%% ISO View +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); + +%% Side view +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) + +%% Top view +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) + +%% Example of a cubic architecture with cube's center above the top platform - Medium cube size +Hc = H + 2*MO_B; % Size of the useful part of the cube [m] + +stewart_medium = initializeStewartPlatform(); +stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B); +stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_medium = computeJointsPose(stewart_medium); +stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1); +stewart_medium = computeJacobian(stewart_medium); +stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb))); + +%% ISO View +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); + +%% Side view +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) + +%% Top view +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) + +%% Example of a cubic architecture with cube's center above the top platform - Large cube size +Hc = 2*(H + MO_B); % Size of the useful part of the cube [m] + +stewart_large = initializeStewartPlatform(); +stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B); +stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_large = computeJointsPose(stewart_large); +stewart_large = initializeStrutDynamics(stewart_large, 'k', 1); +stewart_large = computeJacobian(stewart_large); +stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb))); + +%% ISO View +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); + +%% Side view +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) + +%% Top view +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) + +%% Get the analytical formula for the location of the top and bottom joints +% Define symbolic variables +syms k Hc Hcom alpha H + +assume(k > 0); % k is positive real +assume(Hcom > 0); % k is positive real +assume(Hc > 0); % Hc is real +assume(H > 0); % H is real +assume(alpha, 'real'); % alpha is real + +% Define si matrix (edges of the cubes) +si = 1/sqrt(3)*[ + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1]; ... + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1] ... +]; + +% Define ci matrix (vertices of the cubes) +ci = Hc * [ + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [-sqrt(2), 0, 0.5]; ... + [-sqrt(2), 0, 0.5] ... +]; + +% Apply vertical shift to ci +ci = ci + (H + Hcom) * [0, 0, 1]; + +% Calculate bi vectors (Stewart platform top joints) +bi = ci + alpha * si; + + +% Extract the z-component value from the first row of ci +% (all rows have the same z-component) +ci_z = ci(1, 3); + +% The z-component of si is 1 for all rows +si_z = si(1, 3); + +alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha); +alpha_for_H = solve(ci_z + alpha * si_z == H, alpha); + +% Verify the results +% Substitute alpha values and check the resulting bi_z values +bi_z_0 = ci + alpha_for_0 * si; +disp('Radius for fixed base:'); +simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2)) + +bi_z_H = ci + alpha_for_H * si; +disp('Radius for mobile platform:'); +simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2)) diff --git a/matlab/figs b/matlab/figs new file mode 120000 index 0000000..30fa883 --- /dev/null +++ b/matlab/figs @@ -0,0 +1 @@ +../paper/figs \ No newline at end of file diff --git a/matlab/index.org b/matlab/index.org new file mode 100644 index 0000000..93289a6 --- /dev/null +++ b/matlab/index.org @@ -0,0 +1,1426 @@ +#+TITLE: Decoupling Properties of the Cubic Architecture +:DRAWER: +#+LANGUAGE: en +#+EMAIL: dehaeze.thomas@gmail.com +#+AUTHOR: Dehaeze Thomas + +#+PROPERTY: header-args:matlab :session *MATLAB* +#+PROPERTY: header-args:matlab+ :comments no +#+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 dehaeze26_cubic_architecture.m +:END: + +* 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 :noweb yes :results silent +<> +#+end_src + +#+begin_src matlab :noweb yes :results silent +<> +#+end_src + +#+begin_src matlab :noweb yes :results silent +<> +#+end_src + +* Introduction :ignore: + +The Cubic configuration for the Stewart platform was first proposed by Dr. Gough in a comment to the original paper by Dr. Stewart [[cite:&stewart65_platf_with_six_degrees_freed]]. +This configuration is characterized by active struts arranged in a mutually orthogonal configuration connecting the corners of a cube, as shown in Figure ref:fig:detail_kinematics_cubic_architecture_example. + +Typically, the struts have similar length to the cube's edges, as illustrated in Figure ref:fig:detail_kinematics_cubic_architecture_example. +Practical implementations of such configurations can be observed in Figures ref:fig:detail_kinematics_jpl, ref:fig:detail_kinematics_uw_gsp and ref:fig:detail_kinematics_uqp. +It is also possible to implement designs with strut lengths smaller than the cube's edges (Figure ref:fig:detail_kinematics_cubic_architecture_example_small), as exemplified in Figure ref:fig:detail_kinematics_ulb_pz. + +#+begin_src matlab :exports none :results none +%% Example of a typical "cubic" architecture +MO_B = -50e-3; % Position {B} with respect to {M} [m] + +H = 100e-3; % Height of the Stewart platform [m] + +Hc = 100e-3; % Size of the useful part of the cube [m] + +FOc = H + MO_B; % Center of the cube with respect to {F} + % here positionned at the frame {B} + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1); +stewart = initializeJointDynamics(stewart); +stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); +stewart = initializeCylindricalStruts(stewart); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); +view([40, 5]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_architecture_example.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+begin_src matlab :exports none :results none +%% Example of a typical "cubic" architecture +MO_B = -20e-3; % Position {B} with respect to {M} [m] + +H = 40e-3; % Height of the Stewart platform [m] + +Hc = 100e-3; % Size of the useful part of the cube [m] + +FOc = H + MO_B; % Center of the cube with respect to {F} + % here positionned at the frame {B} + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1); +stewart = computeJacobian(stewart); +stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 150e-3, 'Mpr', 150e-3); + +displayArchitecture(stewart, 'labels', false, 'frames', false); +plotCube(stewart, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', false); +view([40, 5]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_architecture_example_small.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+name: fig:detail_kinematics_cubic_architecture_examples +#+caption: Typical Stewart platform cubic architectures in which struts' length is similar to the cube edges's length (\subref{fig:detail_kinematics_cubic_architecture_example}) or is taking just a portion of the edge (\subref{fig:detail_kinematics_cubic_architecture_example_small}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example}Classical Cubic architecture} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_architecture_example.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example_small}Alternative configuration} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_architecture_example_small.png]] +#+end_subfigure +#+end_figure + + +Several advantageous properties attributed to the cubic configuration have contributed to its widespread adoption [[cite:&geng94_six_degree_of_freed_activ;&preumont07_six_axis_singl_stage_activ;&jafari03_orthog_gough_stewar_platf_microm]]: simplified kinematics relationships and dynamical analysis [[cite:&geng94_six_degree_of_freed_activ]]; uniform stiffness in all directions [[cite:&hanieh03_activ_stewar]]; uniform mobility [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition, chapt.8.5.2]]; and minimization of the cross coupling between actuators and sensors in different struts [[cite:&preumont07_six_axis_singl_stage_activ]]. +This minimization is attributed to the fact that the struts are orthogonal to each other, and is said to facilitate collocated sensor-actuator control system design, i.e., the implementation of decentralized control [[cite:&geng94_six_degree_of_freed_activ;&thayer02_six_axis_vibrat_isolat_system]]. + +These properties are examined in this section to assess their relevance for the nano-hexapod. +The mobility and stiffness properties of the cubic configuration are analyzed in Section ref:ssec:detail_kinematics_cubic_static. +Dynamical decoupling is investigated in Section ref:ssec:detail_kinematics_cubic_dynamic, while decentralized control, crucial for the NASS, is examined in Section ref:ssec:detail_kinematics_decentralized_control. +Given that the cubic architecture imposes strict geometric constraints, alternative designs are proposed in Section ref:ssec:detail_kinematics_cubic_design. +The ultimate objective is to determine the suitability of the cubic architecture for the nano-hexapod. + +* Static Properties +<> +*** Stiffness matrix for the Cubic architecture + +Consider the cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_schematic_full. +The unit vectors corresponding to the edges of the cube are described by equation eqref:eq:detail_kinematics_cubic_s. + +\begin{equation}\label{eq:detail_kinematics_cubic_s} + \hat{\bm{s}}_1 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_2 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_3 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_4 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_5 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_6 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} +\end{equation} + +#+name: fig:detail_kinematics_cubic_schematic_cases +#+caption: Cubic architecture. Struts are represented in blue. The cube's center by a black dot. The Struts can match the cube's edges (\subref{fig:detail_kinematics_cubic_schematic_full}) or just take a portion of the edge (\subref{fig:detail_kinematics_cubic_schematic}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic_full}Full cube} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 0.9 +[[file:figs/detail_kinematics_cubic_schematic_full.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic}Cube's portion} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 0.9 +[[file:figs/detail_kinematics_cubic_schematic.png]] +#+end_subfigure +#+end_figure + +Coordinates of the cube's vertices relevant for the top joints, expressed with respect to the cube's center, are shown in equation eqref:eq:detail_kinematics_cubic_vertices. + +\begin{equation}\label{eq:detail_kinematics_cubic_vertices} + \tilde{\bm{b}}_1 = \tilde{\bm{b}}_2 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{-\sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_3 = \tilde{\bm{b}}_4 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{ \sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_5 = \tilde{\bm{b}}_6 = H_c \begin{bmatrix} \frac{-2}{\sqrt{2}} \\ 0 \\ \frac{1}{2} \end{bmatrix} +\end{equation} + +In the case where top joints are positioned at the cube's vertices, a diagonal stiffness matrix is obtained as shown in equation eqref:eq:detail_kinematics_cubic_stiffness. +Translation stiffness is twice the stiffness of the struts, and rotational stiffness is proportional to the square of the cube's size $H_c$. + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness} + \bm{K}_{\{B\} = \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 2 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 & 0 \\ + 0 & 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +However, typically, the top joints are not placed at the cube's vertices but at positions along the cube's edges (Figure ref:fig:detail_kinematics_cubic_schematic). +In that case, the location of the top joints can be expressed by equation eqref:eq:detail_kinematics_cubic_edges, yet the computed stiffness matrix remains identical to Equation eqref:eq:detail_kinematics_cubic_stiffness. + +\begin{equation}\label{eq:detail_kinematics_cubic_edges} + \bm{b}_i = \tilde{\bm{b}}_i + \alpha \hat{\bm{s}}_i +\end{equation} + + +The stiffness matrix is therefore diagonal when the considered $\{B\}$ frame is located at the center of the cube (shown by frame $\{C\}$). +This means that static forces (resp torques) applied at the cube's center will induce pure translations (resp rotations around the cube's center). +This specific location where the stiffness matrix is diagonal is referred to as the "Center of Stiffness" (analogous to the "Center of Mass" where the mass matrix is diagonal). + +#+begin_src matlab +%% Analytical formula for Stiffness matrix of the Cubic Stewart platform +% Define symbolic variables +syms k Hc alpha H + +assume(k > 0); % k is positive real +assume(Hc, 'real'); % Hc is real +assume(H, 'real'); % H is real +assume(alpha, 'real'); % alpha is real + +% Define si matrix (edges of the cubes) +si = 1/sqrt(3)*[ + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1]; ... + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1] ... +]; + +% Define ci matrix (vertices of the cubes) +ci = Hc * [ + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [-sqrt(2), 0, 0.5]; ... + [-sqrt(2), 0, 0.5] ... +]; + +% Apply vertical shift to ci +ci = ci + H * [0, 0, 1]; + +% Calculate bi vectors (Stewart platform top joints) +bi = ci + alpha * si; + +% Initialize stiffness matrix +K = sym(zeros(6,6)); + +% Calculate elements of the stiffness matrix +for i = 1:6 + % Extract vectors for each leg + s_i = si(i,:)'; + b_i = bi(i,:)'; + + % Calculate cross product vector + cross_bs = cross(b_i, s_i); + + % Build matrix blocks + K(1:3, 4:6) = K(1:3, 4:6) + s_i * cross_bs'; + K(4:6, 1:3) = K(4:6, 1:3) + cross_bs * s_i'; + K(1:3, 1:3) = K(1:3, 1:3) + s_i * s_i'; + K(4:6, 4:6) = K(4:6, 4:6) + cross_bs * cross_bs'; +end + +% Scale by stiffness coefficient +K = k * K; + +% Simplify the expressions +K = simplify(K); + +% Display the analytical stiffness matrix +disp('Analytical Stiffness Matrix:'); +pretty(K); +#+end_src + +*** Effect of having frame $\{B\}$ off-centered + +When the reference frames $\{A\}$ and $\{B\}$ are shifted from the cube's center, off-diagonal elements emerge in the stiffness matrix. + +Considering a vertical shift as shown in Figure ref:fig:detail_kinematics_cubic_schematic, the stiffness matrix transforms into that shown in Equation eqref:eq:detail_kinematics_cubic_stiffness_off_centered. +Off-diagonal elements increase proportionally with the height difference between the cube's center and the considered $\{B\}$ frame. + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness_off_centered} + \bm{K}_{\{B\} \neq \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & -2 H & 0 \\ + 0 & 2 & 0 & 2 H & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 2 H & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 & 0 \\ + -2 H & 0 & 0 & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +This stiffness matrix structure is characteristic of Stewart platforms exhibiting symmetry, and is not an exclusive property of cubic architectures. +Therefore, the stiffness characteristics of the cubic architecture are only distinctive when considering a reference frame located at the cube's center. +This poses a practical limitation, as in most applications, the relevant frame (where motion is of interest and forces are applied) is located above the top platform. + +It should be noted that for the stiffness matrix to be diagonal, the cube's center doesn't need to coincide with the geometric center of the Stewart platform. +This observation leads to the interesting alternative architectures presented in Section ref:ssec:detail_kinematics_cubic_design. + +*** Uniform Mobility + +The translational mobility of the Stewart platform with constant orientation was analyzed. +Considering limited actuator stroke (elongation of each strut), the maximum achievable positions in XYZ space were estimated. +The resulting mobility in X, Y, and Z directions for the cubic architecture is illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_translations. + +The translational workspace analysis reveals that for the cubic architecture, the achievable positions form a cube whose axes align with the struts, with the cube's edge length corresponding to the strut axial stroke. +These findings suggest that the mobility pattern is more subtle than sometimes described in the literature [[cite:&mcinroy00_desig_contr_flexur_joint_hexap]], exhibiting uniformity primarily along directions aligned with the cube's edges rather than uniform spherical distribution in all XYZ directions. +This configuration still offers more consistent mobility characteristics compared to alternative architectures illustrated in Figure ref:fig:detail_kinematics_mobility_trans. + +The rotational mobility, illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_rotations, exhibits greater achievable angular stroke in the $R_x$ and $R_y$ directions compared to the $R_z$ direction. +Furthermore, an inverse relationship exists between the cube's dimension and rotational mobility, with larger cube sizes corresponding to more limited angular displacement capabilities. + +#+begin_src matlab +%% Cubic configuration +H = 100e-3; % Height of the Stewart platform [m] +Hc = 100e-3; % Size of the useful part of the cube [m] +FOc = 50e-3; % Center of the cube at the Stewart platform center +MO_B = -50e-3; % Position {B} with respect to {M} [m] +MHb = 0; + +stewart_cubic = initializeStewartPlatform(); +stewart_cubic = initializeFramesPositions(stewart_cubic, 'H', H, 'MO_B', MO_B); +stewart_cubic = generateCubicConfiguration(stewart_cubic, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', MHb); +stewart_cubic = computeJointsPose(stewart_cubic); +stewart_cubic = initializeStrutDynamics(stewart_cubic, 'k', 1); +stewart_cubic = computeJacobian(stewart_cubic); +stewart_cubic = initializeCylindricalPlatforms(stewart_cubic, 'Fpr', 150e-3, 'Mpr', 150e-3); + +% Let's now define the actuator stroke. +L_max = 50e-6; % [m] +#+end_src + +#+begin_src matlab :exports none :results none +%% Mobility of a Stewart platform with Cubic architecture - Translations +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 200); +rs = zeros(length(thetas), length(phis)); + +for i = 1:length(thetas) + for j = 1:length(phis) + Tx = sin(thetas(i))*cos(phis(j)); + Ty = sin(thetas(i))*sin(phis(j)); + Tz = cos(thetas(i)); + + dL = stewart_cubic.kinematics.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction + + rs(i, j) = L_max/max(abs(dL)); + % rs(i, j) = max(abs([dL(dL<0)*L_min; dL(dL>=0)*L_max])); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); +X = 1e6 * rs .* sin(theta_grid) .* cos(phi_grid); +Y = 1e6 * rs .* sin(theta_grid) .* sin(phi_grid); +Z = 1e6 * rs .* cos(theta_grid); + +figure; +hold on; +surf(X, Y, Z, 'FaceColor', 'white', 'EdgeColor', colors(1,:)); +quiver3(0, 0, 0, 150, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 150, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 0, 150, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(150, 0, 0, '$D_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 150, 0, '$D_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 150, '$D_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_mobility_translations.pdf', 'width', 'normal', 'height', 'full', 'simplify', true); +#+end_src + +#+begin_src matlab :exports none :results none +%% Mobility of a Stewart platform with Cubic architecture - Rotations +thetas = linspace(0, pi, 100); +phis = linspace(0, 2*pi, 200); +rs_cubic = zeros(length(thetas), length(phis)); + +for i = 1:length(thetas) + for j = 1:length(phis) + Rx = sin(thetas(i))*cos(phis(j)); + Ry = sin(thetas(i))*sin(phis(j)); + Rz = cos(thetas(i)); + + dL = stewart_cubic.kinematics.J*[0; 0; 0; Rx; Ry; Rz;]; + rs_cubic(i, j) = L_max/max(abs(dL)); + end +end + +[phi_grid, theta_grid] = meshgrid(phis, thetas); + +X_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* cos(phi_grid); +Y_cubic = 1e6 * rs_cubic .* sin(theta_grid) .* sin(phi_grid); +Z_cubic = 1e6 * rs_cubic .* cos(theta_grid); + +figure; +hold on; +surf(X_cubic, Y_cubic, Z_cubic, 'FaceColor', 'white', 'LineWidth', 0.2, 'EdgeColor', colors(1,:)); +quiver3(0, 0, 0, 1500, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 1500, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +quiver3(0, 0, 0, 0, 0, 1500, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.7); +text(1500, 0, 0, '$R_x$', 'FontSize', 10, 'HorizontalAlignment', 'center', 'VerticalAlignment', 'top' ); +text(0, 1500, 0, '$R_y$', 'FontSize', 10, 'HorizontalAlignment', 'right', 'VerticalAlignment', 'bottom'); +text(0, 0, 1500, '$R_z$', 'FontSize', 10, 'HorizontalAlignment', 'left', 'VerticalAlignment', 'top' ); +hold off; +axis equal; +grid off; +axis off; +view(105, 15); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_mobility_rotations.pdf', 'width', 'normal', 'height', 'full', 'simplify', true); +#+end_src + +#+name: fig:detail_kinematics_cubic_mobility +#+caption: Mobility of a Stewart platform with Cubic architecture. Both for translations (\subref{fig:detail_kinematics_cubic_mobility_translations}) and rotations (\subref{fig:detail_kinematics_cubic_mobility_rotations}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_translations}Mobility in translation} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_mobility_translations.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_rotations}Mobility in rotation} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_mobility_rotations.png]] +#+end_subfigure +#+end_figure + +* Dynamical Decoupling +<> +*** Introduction :ignore: + +This section examines the dynamics of the cubic architecture in the Cartesian frame which corresponds to the transfer function from forces and torques $\bm{\mathcal{F}}$ to translations and rotations $\bm{\mathcal{X}}$ of the top platform. +When relative motion sensors are integrated in each strut (measuring $\bm{\mathcal{L}}$), the pose $\bm{\mathcal{X}}$ is computed using the Jacobian matrix as shown in Figure ref:fig:detail_kinematics_centralized_control. + +#+name: fig:detail_kinematics_centralized_control +#+caption: Typical control architecture in the cartesian frame +[[file:figs/detail_kinematics_centralized_control.png]] + +*** Low frequency and High frequency coupling + +As derived during the conceptual design phase, the dynamics from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ is described by Equation eqref:eq:detail_kinematics_transfer_function_cart. +At low frequency, the behavior of the platform depends on the stiffness matrix eqref:eq:detail_kinematics_transfer_function_cart_low_freq. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_cart_low_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to 0]{} \bm{K}^{-1} +\end{equation} + +In Section ref:ssec:detail_kinematics_cubic_static, it was demonstrated that for the cubic configuration, the stiffness matrix is diagonal if frame $\{B\}$ is positioned at the cube's center. +In this case, the "Cartesian" plant is decoupled at low frequency. +At high frequency, the behavior is governed by the mass matrix (evaluated at frame $\{B\}$) eqref:eq:detail_kinematics_transfer_function_high_freq. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_high_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to \infty]{} - \omega^2 \bm{M}^{-1} +\end{equation} + +To achieve a diagonal mass matrix, the center of mass of the mobile components must coincide with the $\{B\}$ frame, and the principal axes of inertia must align with the axes of the $\{B\}$ frame. + +#+name: fig:detail_kinematics_cubic_payload +#+caption: Cubic stewart platform with top cylindrical payload +#+attr_latex: :width 0.6\linewidth +[[file:figs/detail_kinematics_cubic_payload.png]] + +To verify these properties, a cubic Stewart platform with a cylindrical payload was analyzed (Figure ref:fig:detail_kinematics_cubic_payload). +Transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ were computed for two specific locations of the $\{B\}$ frames. +When the $\{B\}$ frame was positioned at the center of mass, coupling at low frequency was observed due to the non-diagonal stiffness matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com). +Conversely, when positioned at the center of stiffness, coupling occurred at high frequency due to the non-diagonal mass matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_cok). + +#+begin_src matlab +%% Input/Output definition of the Simscape model +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External metrology [m,rad] + +% Prepare simulation +controller = initializeController('type', 'open-loop'); +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); + +%% Cubic Stewart platform with payload above the top platform - B frame at the CoM +H = 200e-3; % height of the Stewart platform [m] +MO_B = 50e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_CoM = linearize(mdl, io)*inv(stewart.kinematics.J).'; +G_CoM.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoM.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; + +%% Same geometry but B Frame at cube's center (CoK) +MO_B = -100e-3; % Position {B} with respect to {M} [m] +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); +G_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +#+end_src + +#+begin_src matlab :exports none :results none +%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of mass of the payload +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +% for i = 1:5 +% for j = i+1:6 +% plot(freqs, abs(squeeze(freqresp(G_CoM(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... +% 'HandleVisibility', 'off'); +% end +% end +plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude'); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_cart_coupling_com.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+begin_src matlab :exports none :results none +%% Coupling in the cartesian frame for a Cubic Stewart platform - Frame {B} is at the center of the cube +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +% for i = 1:5 +% for j = i+1:6 +% plot(freqs, abs(squeeze(freqresp(G_CoK(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... +% 'HandleVisibility', 'off'); +% end +% end +plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude'); +leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_cart_coupling_cok.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+name: fig:detail_kinematics_cubic_cart_coupling +#+caption: Transfer functions for a Cubic Stewart platform expressed in the Cartesian frame. Two locations of the $\{B\}$ frame are considered: at the center of mass of the moving body (\subref{fig:detail_kinematics_cubic_cart_coupling_com}) and at the cube's center (\subref{fig:detail_kinematics_cubic_cart_coupling_cok}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com}$\{B\}$ at the center of mass} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_com.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_cok}$\{B\}$ at the cube's center} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_cok.png]] +#+end_subfigure +#+end_figure + +*** Payload's CoM at the cube's center + +An effective strategy for improving dynamical performances involves aligning the cube's center (center of stiffness) with the center of mass of the moving components [[cite:&li01_simul_fault_vibrat_isolat_point]]. +This can be achieved by positioning the payload below the top platform, such that the center of mass of the moving body coincides with the cube's center (Figure ref:fig:detail_kinematics_cubic_centered_payload). +This approach was physically implemented in several studies [[cite:&mcinroy99_dynam;&jafari03_orthog_gough_stewar_platf_microm]], as shown in Figure ref:fig:detail_kinematics_uw_gsp. +The resulting dynamics are indeed well-decoupled (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com_cok), taking advantage from diagonal stiffness and mass matrices. +The primary limitation of this approach is that, for many applications including the nano-hexapod, the payload must be positioned above the top platform. +If a design similar to Figure ref:fig:detail_kinematics_cubic_centered_payload were employed for the nano-hexapod, the X-ray beam would intersect with the struts during spindle rotation. + +#+begin_src matlab +%% Cubic Stewart platform with payload above the top platform +H = 200e-3; +MO_B = -100e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Sample at the Center of the cube +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'H_offset', -H/2-50e-3); + +% Run the linearization +G_CoM_CoK = linearize(mdl, io)*inv(stewart.kinematics.J.'); +G_CoM_CoK.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; +G_CoM_CoK.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; +#+end_src + +#+begin_src matlab :exports none +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ... + 'DisplayName', '$D_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ... + 'DisplayName', '$D_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ... + 'DisplayName', '$D_z/F_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 4), freqs, 'Hz'))), 'color', colors(4,:), ... + 'DisplayName', '$R_x/M_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 5), freqs, 'Hz'))), 'color', colors(5,:), ... + 'DisplayName', '$R_y/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(6, 6), freqs, 'Hz'))), 'color', colors(6,:), ... + 'DisplayName', '$R_z/M_z$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(4, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_x/F_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(5, 1), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$R_y/F_x$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(1, 5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_x/M_y$'); +plot(freqs, abs(squeeze(freqresp(G_CoM_CoK(2, 4), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'DisplayName', '$D_y/M_x$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); +leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 2e-3]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_cart_coupling_com_cok.pdf', 'width', 'half', 'height', 600); +#+end_src + +#+name: fig:detail_kinematics_cubic_com_cok +#+caption: Cubic Stewart platform with payload at the cube's center (\subref{fig:detail_kinematics_cubic_centered_payload}). Obtained cartesian plant is fully decoupled (\subref{fig:detail_kinematics_cubic_cart_coupling_com_cok}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_centered_payload}Payload at the cube's center} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_centered_payload.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com_cok}Fully decoupled cartesian plant} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_com_cok.png]] +#+end_subfigure +#+end_figure + +*** Conclusion + +The analysis of dynamical properties of the cubic architecture yields several important conclusions. +Static decoupling, characterized by a diagonal stiffness matrix, is achieved when reference frames $\{A\}$ and $\{B\}$ are positioned at the cube's center. +Note that this property can also be obtained with non-cubic architectures that exhibit symmetrical strut arrangements. +Dynamic decoupling requires both static decoupling and coincidence of the mobile platform's center of mass with reference frame $\{B\}$. +While this configuration offers powerful control advantages, it requires positioning the payload at the cube's center, which is highly restrictive and often impractical. + +* Decentralized Control +<> +*** Introduction :ignore: + +The orthogonal arrangement of struts in the cubic architecture suggests a potential minimization of inter-strut coupling, which could theoretically create favorable conditions for decentralized control. +Two sensor types integrated in the struts are considered: displacement sensors and force sensors. +The control architecture is illustrated in Figure ref:fig:detail_kinematics_decentralized_control, where $\bm{K}_{\mathcal{L}}$ represents a diagonal transfer function matrix. + +#+name: fig:detail_kinematics_decentralized_control +#+caption: Decentralized control in the frame of the struts. +[[file:figs/detail_kinematics_decentralized_control.png]] + +The obtained plant dynamics in the frame of the struts are compared for two Stewart platforms. +The first employs a cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_payload. +The second uses a non-cubic Stewart platform shown in Figure ref:fig:detail_kinematics_non_cubic_payload, featuring identical payload and strut dynamics but with struts oriented more vertically to differentiate it from the cubic architecture. + +#+name: fig:detail_kinematics_non_cubic_payload +#+caption: Stewart platform with non-cubic architecture +#+attr_latex: :width 0.6\linewidth +[[file:figs/detail_kinematics_non_cubic_payload.png]] + +#+begin_src matlab +%% Input/Output definition of the Simscape model +clear io; io_i = 1; +io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Displacement sensors [m] +io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensor [N] + +% Prepare simulation : Payload above the top platform +controller = initializeController('type', 'open-loop'); +sample = initializeSample('type', 'cylindrical', 'm', 10, 'H', 100e-3, 'R', 100e-3); + +%% Cubic Stewart platform +H = 200e-3; % height of the Stewart platform [m] +MO_B = 50e-3; % Position {B} with respect to {M} [m] + +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_cubic = linearize(mdl, io); +G_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; + +%% Non-Cubic Stewart platform +stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', H, 'MO_B', MO_B); +stewart = generateCubicConfiguration(stewart, 'Hc', H, 'FOc', H/2, 'FHa', 25e-3, 'MHb', 25e-3); +stewart = generateGeneralConfiguration(stewart, 'FH', 25e-3, 'FR', 250e-3, 'MH', 25e-3, 'MR', 250e-3, ... + 'FTh', [-22, 22, 120-22, 120+22, 240-22, 240+22]*(pi/180), ... + 'MTh', [-60+22, 60-22, 60+22, 180-22, 180+22, -60-22]*(pi/180)); +stewart = computeJointsPose(stewart); +stewart = initializeStrutDynamics(stewart, 'k', 1e6, 'c', 1e1); +stewart = initializeJointDynamics(stewart, 'type_F', '2dof', 'type_M', '3dof'); +stewart = computeJacobian(stewart); +stewart = initializeStewartPose(stewart); +stewart = initializeCylindricalPlatforms(stewart, ... + 'Mpm', 1e-6, ... % Massless platform + 'Fpm', 1e-6, ... % Massless platform + 'Mph', 20e-3, ... % Thin platform + 'Fph', 20e-3, ... % Thin platform + 'Mpr', 1.2*max(vecnorm(stewart.platform_M.Mb)), ... + 'Fpr', 1.2*max(vecnorm(stewart.platform_F.Fa))); +stewart = initializeCylindricalStruts(stewart, ... + 'Fsm', 1e-6, ... % Massless strut + 'Msm', 1e-6, ... % Massless strut + 'Fsh', stewart.geometry.l(1)/2, ... + 'Msh', stewart.geometry.l(1)/2 ... +); + +% Run the linearization +G_non_cubic = linearize(mdl, io); +G_non_cubic.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'}; +G_non_cubic.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ... + 'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'}; +#+end_src + +*** Relative Displacement Sensors + +The transfer functions from actuator force in each strut to the relative motion of the struts are presented in Figure ref:fig:detail_kinematics_decentralized_dL. +As anticipated from the equations of motion from $\bm{f}$ to $\bm{\mathcal{L}}$ eqref:eq:detail_kinematics_transfer_function_struts, the $6 \times 6$ plant is decoupled at low frequency. +At high frequency, coupling is observed as the mass matrix projected in the strut frame is not diagonal. + +No significant advantage is evident for the cubic architecture (Figure ref:fig:detail_kinematics_cubic_decentralized_dL) compared to the non-cubic architecture (Figure ref:fig:detail_kinematics_non_cubic_decentralized_dL). +The resonance frequencies differ between the two cases because the more vertical strut orientation in the non-cubic architecture alters the stiffness properties of the Stewart platform, consequently shifting the frequencies of various modes. + +#+begin_src matlab :exports none :results none +%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... + 'DisplayName', '$l_i/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_non_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'DisplayName', '$l_i/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 1e-4]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_non_cubic_decentralized_dL.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Decentralized plant - Actuator force to Strut displacement - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('dL%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_cubic('dL1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... + 'DisplayName', '$l_i/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_cubic('dL2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'DisplayName', '$l_i/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); +ylim([1e-10, 1e-4]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_decentralized_dL.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+name: fig:detail_kinematics_decentralized_dL +#+caption: Bode plot of the transfer functions from actuator force to relative displacement sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_dL}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_dL}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_dL}Non cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_non_cubic_decentralized_dL.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_dL}Cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_decentralized_dL.png]] +#+end_subfigure +#+end_figure + +*** Force Sensors + +Similarly, the transfer functions from actuator force to force sensors in each strut were analyzed for both cubic and non-cubic Stewart platforms. +The results are presented in Figure ref:fig:detail_kinematics_decentralized_fn. +The system demonstrates good decoupling at high frequency in both cases, with no clear advantage for the cubic architecture. + +#+begin_src matlab :exports none :results none +%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_non_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:)], 'linewidth', 2.5, ... + 'DisplayName', '$f_{m,i}/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_non_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(1,:), 0.1], ... + 'DisplayName', '$f_{m,i}/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); ylim([1e-4, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_non_cubic_decentralized_fn.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Decentralized plant - Actuator force to strut force sensor - Cubic Architecture +freqs = logspace(0, 4, 1000); +figure; +tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); +ax1 = nexttile(); +hold on; +for i = 1:5 + for j = i+1:6 + plot(freqs, abs(squeeze(freqresp(G_cubic(sprintf('fn%i',i), sprintf('f%i',j)), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'HandleVisibility', 'off'); + end +end +plot(freqs, abs(squeeze(freqresp(G_cubic('fn1', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:)], 'linewidth', 2.5, ... + 'DisplayName', '$f_{m,i}/f_i$'); +plot(freqs, abs(squeeze(freqresp(G_cubic('fn2', 'f1'), freqs, 'Hz'))), 'color', [colors(2,:), 0.1], ... + 'DisplayName', '$f_{m,i}/f_j$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]'); +leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1); +leg.ItemTokenSize(1) = 15; +xlim([1, 1e4]); ylim([1e-4, 1e2]); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_decentralized_fn.pdf', 'width', 'half', 'height', 'normal'); +#+end_src + +#+name: fig:detail_kinematics_decentralized_fn +#+caption: Bode plot of the transfer functions from actuator force to force sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_fn}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_fn}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_fn}Non cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_non_cubic_decentralized_fn.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_fn}Cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_decentralized_fn.png]] +#+end_subfigure +#+end_figure + +*** Conclusion + +The presented results do not demonstrate the pronounced decoupling advantages often associated with cubic architectures in the literature. +Both the cubic and non-cubic configurations exhibited similar coupling characteristics, suggesting that the benefits of orthogonal strut arrangement for decentralized control is less obvious than often reported in the literature. + +* Cubic architecture with Cube's center above the top platform +<> +*** Introduction :ignore: + +As demonstrated in Section ref:ssec:detail_kinematics_cubic_dynamic, the cubic architecture can exhibit advantageous dynamical properties when the center of mass of the moving body coincides with the cube's center, resulting in diagonal mass and stiffness matrices. +As shown in Section ref:ssec:detail_kinematics_cubic_static, the stiffness matrix is diagonal when the considered $\{B\}$ frame is located at the cube's center. +However, the $\{B\}$ frame is typically positioned above the top platform where forces are applied and displacements are measured. + +This section proposes modifications to the cubic architecture to enable positioning the payload above the top platform while still leveraging the advantageous dynamical properties of the cubic configuration. + +Three key parameters define the geometry of the cubic Stewart platform: $H$, the height of the Stewart platform (distance from fixed base to mobile platform); $H_c$, the height of the cube, as shown in Figure ref:fig:detail_kinematics_cubic_schematic_full; and $H_{CoM}$, the height of the center of mass relative to the mobile platform (coincident with the cube's center). + +Depending on the cube's size $H_c$ in relation to $H$ and $H_{CoM}$, different designs emerge. +In the following examples, $H = 100\,mm$ and $H_{CoM} = 20\,mm$. + +#+begin_src matlab +%% Cubic configurations with center of the cube above the top platform +H = 100e-3; % height of the Stewart platform [m] +MO_B = 20e-3; % Position {B} with respect to {M} [m] +FOc = H + MO_B; % Center of the cube with respect to {F} +#+end_src + +*** Small cube + +When the cube size $H_c$ is smaller than twice the height of the CoM $H_{CoM}$ eqref:eq:detail_kinematics_cube_small, the resulting design is shown in Figure ref:fig:detail_kinematics_cubic_above_small. + +\begin{equation}\label{eq:detail_kinematics_cube_small} + H_c < 2 H_{CoM} +\end{equation} + +# TODO - Add link to Figure ref:fig:nhexa_stewart_piezo_furutani (page pageref:fig:nhexa_stewart_piezo_furutani) +This configuration is similar to that described in [[cite:&furutani04_nanom_cuttin_machin_using_stewar]], although they do not explicitly identify it as a cubic configuration. +Adjacent struts are parallel to each other, differing from the typical architecture where parallel struts are positioned opposite to each other. + +This approach yields a compact architecture, but the small cube size may result in insufficient rotational stiffness. + +#+begin_src matlab +%% Small cube +Hc = 2*MO_B; % Size of the useful part of the cube [m] + +stewart_small = initializeStewartPlatform(); +stewart_small = initializeFramesPositions(stewart_small, 'H', H, 'MO_B', MO_B); +stewart_small = generateCubicConfiguration(stewart_small, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_small = computeJointsPose(stewart_small); +stewart_small = initializeStrutDynamics(stewart_small, 'k', 1); +stewart_small = computeJacobian(stewart_small); +stewart_small = initializeCylindricalPlatforms(stewart_small, 'Fpr', 1.1*max(vecnorm(stewart_small.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_small.platform_M.Mb))); +#+end_src + +#+begin_src matlab :exports none :results none +%% ISO View +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_small_iso.pdf', 'width', 'normal', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Side view +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_small_side.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Top view +displayArchitecture(stewart_small, 'labels', false, 'frames', false); +plotCube(stewart_small, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_small_top.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+name: fig:detail_kinematics_cubic_above_small +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 40mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_top.png]] +#+end_subfigure +#+end_figure + +*** Medium sized cube + +Increasing the cube's size such that eqref:eq:detail_kinematics_cube_medium is verified produces an architecture with intersecting struts (Figure ref:fig:detail_kinematics_cubic_above_medium). + +\begin{equation}\label{eq:detail_kinematics_cube_medium} + 2 H_{CoM} < H_c < 2 (H_{CoM} + H) +\end{equation} + +This configuration resembles the design proposed in [[cite:&yang19_dynam_model_decoup_contr_flexib]] (Figure ref:fig:detail_kinematics_yang19), although their design is not strictly cubic. + +#+begin_src matlab :exports none :results none +%% Example of a cubic architecture with cube's center above the top platform - Medium cube size +Hc = H + 2*MO_B; % Size of the useful part of the cube [m] + +stewart_medium = initializeStewartPlatform(); +stewart_medium = initializeFramesPositions(stewart_medium, 'H', H, 'MO_B', MO_B); +stewart_medium = generateCubicConfiguration(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_medium = computeJointsPose(stewart_medium); +stewart_medium = initializeStrutDynamics(stewart_medium, 'k', 1); +stewart_medium = computeJacobian(stewart_medium); +stewart_medium = initializeCylindricalPlatforms(stewart_medium, 'Fpr', 1.1*max(vecnorm(stewart_medium.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_medium.platform_M.Mb))); +#+end_src + +#+begin_src matlab :exports none :results none +%% ISO View +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_medium_iso.pdf', 'width', 'normal', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Side view +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_medium_side.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Top view +displayArchitecture(stewart_medium, 'labels', false, 'frames', false); +plotCube(stewart_medium, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_medium_top.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+name: fig:detail_kinematics_cubic_above_medium +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 140mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_top.png]] +#+end_subfigure +#+end_figure + +*** Large cube + +When the cube's height exceeds twice the sum of the platform height and CoM height eqref:eq:detail_kinematics_cube_large, the architecture shown in Figure ref:fig:detail_kinematics_cubic_above_large is obtained. + +\begin{equation}\label{eq:detail_kinematics_cube_large} + 2 (H_{CoM} + H) < H_c +\end{equation} + +#+begin_src matlab :exports none :results none +%% Example of a cubic architecture with cube's center above the top platform - Large cube size +Hc = 2*(H + MO_B); % Size of the useful part of the cube [m] + +stewart_large = initializeStewartPlatform(); +stewart_large = initializeFramesPositions(stewart_large, 'H', H, 'MO_B', MO_B); +stewart_large = generateCubicConfiguration(stewart_large, 'Hc', Hc, 'FOc', FOc, 'FHa', 5e-3, 'MHb', 5e-3); +stewart_large = computeJointsPose(stewart_large); +stewart_large = initializeStrutDynamics(stewart_large, 'k', 1); +stewart_large = computeJacobian(stewart_large); +stewart_large = initializeCylindricalPlatforms(stewart_large, 'Fpr', 1.1*max(vecnorm(stewart_large.platform_F.Fa)), 'Mpr', 1.1*max(vecnorm(stewart_large.platform_M.Mb))); +#+end_src + +#+begin_src matlab :exports none :results none +%% ISO View +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0,0,0,0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_large_iso.pdf', 'width', 'normal', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Side view +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([90,0]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_large_side.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+begin_src matlab :exports none :results none +%% Top view +displayArchitecture(stewart_large, 'labels', false, 'frames', false); +plotCube(stewart_large, 'Hc', Hc, 'FOc', FOc, 'color', [0, 0, 0, 0.2], 'link_to_struts', true); +scatter3(0, 0, FOc, 200, 'kh'); +view([0,90]) +#+end_src + +#+begin_src matlab :tangle no :exports results :results file none +exportFig('figs/detail_kinematics_cubic_above_large_top.pdf', 'width', 'half', 'height', 'full'); +#+end_src + +#+name: fig:detail_kinematics_cubic_above_large +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 240mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_top.png]] +#+end_subfigure +#+end_figure + +*** Platform size + +#+begin_src matlab +%% Get the analytical formula for the location of the top and bottom joints +% Define symbolic variables +syms k Hc Hcom alpha H + +assume(k > 0); % k is positive real +assume(Hcom > 0); % k is positive real +assume(Hc > 0); % Hc is real +assume(H > 0); % H is real +assume(alpha, 'real'); % alpha is real + +% Define si matrix (edges of the cubes) +si = 1/sqrt(3)*[ + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1]; ... + [ sqrt(2), 0, 1]; ... + [-sqrt(2)/2, -sqrt(3/2), 1]; ... + [-sqrt(2)/2, sqrt(3/2), 1] ... +]; + +% Define ci matrix (vertices of the cubes) +ci = Hc * [ + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), -sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [1/sqrt(2), sqrt(3)/sqrt(2), 0.5]; ... + [-sqrt(2), 0, 0.5]; ... + [-sqrt(2), 0, 0.5] ... +]; + +% Apply vertical shift to ci +ci = ci + (H + Hcom) * [0, 0, 1]; + +% Calculate bi vectors (Stewart platform top joints) +bi = ci + alpha * si; + + +% Extract the z-component value from the first row of ci +% (all rows have the same z-component) +ci_z = ci(1, 3); + +% The z-component of si is 1 for all rows +si_z = si(1, 3); + +alpha_for_0 = solve(ci_z + alpha * si_z == 0, alpha); +alpha_for_H = solve(ci_z + alpha * si_z == H, alpha); + +% Verify the results +% Substitute alpha values and check the resulting bi_z values +bi_z_0 = ci + alpha_for_0 * si; +disp('Radius for fixed base:'); +simplify(sqrt(bi_z_0(1,1).^2 + bi_z_0(1,2).^2)) + +bi_z_H = ci + alpha_for_H * si; +disp('Radius for mobile platform:'); +simplify(sqrt(bi_z_H(1,1).^2 + bi_z_H(1,2).^2)) +#+end_src + +For the proposed configuration, the top joints $\bm{b}_i$ (resp. the bottom joints $\bm{a}_i$) and are positioned on a circle with radius $R_{b_i}$ (resp. $R_{a_i}$) described by Equation eqref:eq:detail_kinematics_cube_joints. + +\begin{subequations}\label{eq:detail_kinematics_cube_joints} +\begin{align} + R_{b_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 H_{CoM}^2} \label{eq:detail_kinematics_cube_top_joints} \\ + R_{a_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 (H_{CoM} + H)^2} \label{eq:detail_kinematics_cube_bot_joints} +\end{align} +\end{subequations} + +Since the rotational stiffness for the cubic architecture scales with the square of the cube's height eqref:eq:detail_kinematics_cubic_stiffness, the cube's size can be determined based on rotational stiffness requirements. +Subsequently, using Equation eqref:eq:detail_kinematics_cube_joints, the dimensions of the top and bottom platforms can be calculated. + +* Conclusion +:PROPERTIES: +:UNNUMBERED: t +:END: + +The analysis of the cubic architecture for Stewart platforms yielded several important findings. +While the cubic configuration provides uniform stiffness in the XYZ directions, it stiffness property becomes particularly advantageous when forces and torques are applied at the cube's center. +Under these conditions, the stiffness matrix becomes diagonal, resulting in a decoupled Cartesian plant at low frequencies. + +Regarding mobility, the translational capabilities of the cubic configuration exhibit uniformity along the directions of the orthogonal struts, rather than complete uniformity in the Cartesian space. +This understanding refines the characterization of cubic architecture mobility commonly presented in literature. + +The analysis of decentralized control in the frame of the struts revealed more nuanced results than expected. +While cubic architectures are frequently associated with reduced coupling between actuators and sensors, this study showed that these benefits may be more subtle or context-dependent than commonly described. +Under the conditions analyzed, the coupling characteristics of cubic and non-cubic configurations, in the frame of the struts, appeared similar. + +Fully decoupled dynamics in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center. +However, this arrangement presents practical challenges, as the cube's center is traditionally located between the top and bottom platforms, making payload placement problematic for many applications. + +To address this limitation, modified cubic architectures have been proposed with the cube's center positioned above the top platform. +Three distinct configurations have been identified, each with different geometric arrangements but sharing the common characteristic that the cube's center is positioned above the top platform. +This structural modification enables the alignment of the moving body's center of mass with the center of stiffness, resulting in beneficial decoupling properties in the Cartesian frame. + + +* Helping Functions :noexport: +:PROPERTIES: +:HEADER-ARGS:matlab+: :tangle no +:END: + +** Initialize Path +#+NAME: m-init-path-tangle +#+BEGIN_SRC matlab +%% Path for functions, data and scripts +addpath('./src/'); % Path for functions +addpath('./subsystems/'); % Path for Subsystems Simulink files +#+END_SRC + +** Initialize Simscape Model +#+NAME: m-init-simscape +#+begin_src matlab +% Simulink Model name +mdl = 'nano_hexapod_model'; +#+end_src + +** Initialize other elements +#+NAME: m-init-other +#+BEGIN_SRC matlab +%% Colors for the figures +colors = colororder; +#+END_SRC diff --git a/matlab/nano_hexapod_model.slx b/matlab/nano_hexapod_model.slx new file mode 100644 index 0000000..b650045 Binary files /dev/null and b/matlab/nano_hexapod_model.slx differ diff --git a/matlab/src/computeJacobian.m b/matlab/src/computeJacobian.m new file mode 100644 index 0000000..a1c76d5 --- /dev/null +++ b/matlab/src/computeJacobian.m @@ -0,0 +1,35 @@ +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: +% - kinematics.J [6x6] - The Jacobian Matrix +% - kinematics.K [6x6] - The Stiffness Matrix +% - kinematics.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*eye(6); + +J = [As' , cross(Ab, As)']; + +K = J'*Ki*J; + +C = inv(K); + +stewart.kinematics.J = J; +stewart.kinematics.K = K; +stewart.kinematics.C = C; diff --git a/matlab/src/computeJointsPose.m b/matlab/src/computeJointsPose.m new file mode 100644 index 0000000..5520000 --- /dev/null +++ b/matlab/src/computeJointsPose.m @@ -0,0 +1,90 @@ +function [stewart] = computeJointsPose(stewart, args) +% computeJointsPose - +% +% Syntax: [stewart] = computeJointsPose(stewart, args) +% +% 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} +% - 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 - 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} + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) +end + +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]); + +Ab = args.ARB *Bb - repmat(-args.AP, [1, 6]); +Ba = args.ARB'*Aa - repmat( args.AP, [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; diff --git a/matlab/src/describeStewartPlatform.m b/matlab/src/describeStewartPlatform.m new file mode 100644 index 0000000..3be8da3 --- /dev/null +++ b/matlab/src/describeStewartPlatform.m @@ -0,0 +1,79 @@ +function [] = describeStewartPlatform(stewart) +% describeStewartPlatform - Display some text describing the current defined Stewart Platform +% +% Syntax: [] = describeStewartPlatform(args) +% +% Inputs: +% - stewart +% +% Outputs: + +arguments + stewart +end + +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') + +fprintf('ACTUATORS:\n') +if stewart.actuators.type == 1 + fprintf('- The actuators are classical.\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, stewart.actuators.c) +elseif stewart.actuators.type == 2 + fprintf('- The actuators are mechanicaly amplified.\n') +end +fprintf('\n') + +fprintf('JOINTS:\n') + +switch stewart.joints_F.type + case 1 + fprintf('- The joints on the fixed based are universal joints\n') + case 2 + fprintf('- The joints on the fixed based are spherical joints\n') + case 3 + fprintf('- The joints on the fixed based are perfect universal joints\n') + case 4 + fprintf('- The joints on the fixed based are perfect spherical joints\n') +end + +switch stewart.joints_M.type + case 1 + fprintf('- The joints on the mobile based are universal joints\n') + case 2 + fprintf('- The joints on the mobile based are spherical joints\n') + case 3 + fprintf('- The joints on the mobile based are perfect universal joints\n') + case 4 + fprintf('- The joints on the mobile based are perfect spherical joints\n') +end + +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) + +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') + +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 diff --git a/matlab/src/displayArchitecture.m b/matlab/src/displayArchitecture.m new file mode 100644 index 0000000..8b66821 --- /dev/null +++ b/matlab/src/displayArchitecture.m @@ -0,0 +1,252 @@ +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: + +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 + +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; + +% The reference frame of the 3d plot corresponds to the frame $\{F\}$. +if ~strcmp(args.views, 'all') + figure; +else + f = figure('visible', 'off'); +end + +hold on; + +% We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$. +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; + +% Let's define a parameter that define the length of the unit vectors used to display the frames. +d_unit_vector = H/4; + +% Let's define a parameter used to position the labels with respect to the center of the element. +d_label = H/20; +% Let's first plot the frame $\{F\}$. +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 + +% Now plot the frame $\{A\}$ fixed to the Base. +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 + +% Let's then plot the circle corresponding to the shape of the Fixed base. +if args.platforms && stewart.platform_F.type == 1 + theta = [0:0.1:2*pi+0.1]; % 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 + +% Let's now plot the position and labels of the Fixed Joints +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 + +% Plot the frame $\{M\}$. +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 + +% Plot the frame $\{B\}$. +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 + +% Let's then plot the circle corresponding to the shape of the Mobile platform. +if args.platforms && stewart.platform_M.type == 1 + theta = [0:0.1:2*pi+0.1]; % 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 + +% Plot the position and labels of the rotation joints fixed to the mobile platform. +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 + +% Plot the legs connecting the joints of the fixed base to the joints of the mobile platform. +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 + +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; + +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 diff --git a/matlab/src/forwardKinematicsApprox.m b/matlab/src/forwardKinematicsApprox.m new file mode 100644 index 0000000..a6f3c23 --- /dev/null +++ b/matlab/src/forwardKinematicsApprox.m @@ -0,0 +1,34 @@ +function [P, R] = forwardKinematicsApprox(stewart, args) +% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using +% the Jacobian Matrix +% +% Syntax: [P, R] = forwardKinematicsApprox(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - kinematics.J [6x6] - The Jacobian Matrix +% - args - Can have the following fields: +% - dL [6x1] - Displacement of each strut [m] +% +% Outputs: +% - P [3x1] - The estimated position of {B} with respect to {A} +% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A} + +arguments + stewart + args.dL (6,1) double {mustBeNumeric} = zeros(6,1) +end + +assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') +J = stewart.kinematics.J; + +X = J\args.dL; + +P = X(1:3); + +theta = norm(X(4:6)); +s = X(4:6)/theta; + +R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta); + s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta); + s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)]; diff --git a/matlab/src/generateCubicConfiguration.m b/matlab/src/generateCubicConfiguration.m new file mode 100644 index 0000000..d08b019 --- /dev/null +++ b/matlab/src/generateCubicConfiguration.m @@ -0,0 +1,57 @@ +function [stewart] = generateCubicConfiguration(stewart, args) +% generateCubicConfiguration - Generate a Cubic Configuration +% +% Syntax: [stewart] = generateCubicConfiguration(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - geometry.H [1x1] - Total height of the platform [m] +% - args - Can have the following fields: +% - Hc [1x1] - Height of the "useful" part of the cube [m] +% - FOc [1x1] - Height of the center of the cube with respect to {F} [m] +% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m] +% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m] +% +% Outputs: + +% - stewart - updated Stewart structure with the added fields: +% - 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.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 + args.FOc (1,1) double {mustBeNumeric} = 50e-3 + args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 + args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 +end + +assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') +H = stewart.geometry.H; + +% We define the useful points of the cube with respect to the Cube's center. +% ${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is located at the center of the cube and aligned with {F} and {M}. + +sx = [ 2; -1; -1]; +sy = [ 0; 1; -1]; +sz = [ 1; 1; 1]; + +R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + +L = args.Hc*sqrt(3); + +Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc]; + +CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg +CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg + +% We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$). + +CSi = (CCm - CCf)./vecnorm(CCm - CCf); + +% We now which to compute the position of the joints $a_{i}$ and $b_{i}$. +Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; +Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; + +stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; diff --git a/matlab/src/generateGeneralConfiguration.m b/matlab/src/generateGeneralConfiguration.m new file mode 100644 index 0000000..4cfcc24 --- /dev/null +++ b/matlab/src/generateGeneralConfiguration.m @@ -0,0 +1,39 @@ +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, mustBeNonnegative} = 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, mustBeNonnegative} = 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; diff --git a/matlab/src/initializeController.m b/matlab/src/initializeController.m new file mode 100644 index 0000000..d0e0025 --- /dev/null +++ b/matlab/src/initializeController.m @@ -0,0 +1,17 @@ +function [controller] = initializeController(args) + + arguments + args.type char {mustBeMember(args.type,{'open-loop', 'iff'})} = 'open-loop' + end + + controller = struct(); + + switch args.type + case 'open-loop' + controller.type = 1; + controller.name = 'Open-Loop'; + case 'iff' + controller.type = 2; + controller.name = 'Decentralized Integral Force Feedback'; + end +end diff --git a/matlab/src/initializeCylindricalPlatforms.m b/matlab/src/initializeCylindricalPlatforms.m new file mode 100644 index 0000000..0b86fc1 --- /dev/null +++ b/matlab/src/initializeCylindricalPlatforms.m @@ -0,0 +1,59 @@ +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; diff --git a/matlab/src/initializeCylindricalStruts.m b/matlab/src/initializeCylindricalStruts.m new file mode 100644 index 0000000..16d4fd2 --- /dev/null +++ b/matlab/src/initializeCylindricalStruts.m @@ -0,0 +1,50 @@ +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; + + 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 diff --git a/matlab/src/initializeFramesPositions.m b/matlab/src/initializeFramesPositions.m new file mode 100644 index 0000000..f68b97d --- /dev/null +++ b/matlab/src/initializeFramesPositions.m @@ -0,0 +1,35 @@ +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; diff --git a/matlab/src/initializeJointDynamics.m b/matlab/src/initializeJointDynamics.m new file mode 100644 index 0000000..4c4d1b5 --- /dev/null +++ b/matlab/src/initializeJointDynamics.m @@ -0,0 +1,129 @@ +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 diff --git a/matlab/src/initializeSample.m b/matlab/src/initializeSample.m new file mode 100644 index 0000000..a0b3eaa --- /dev/null +++ b/matlab/src/initializeSample.m @@ -0,0 +1,26 @@ +function [sample] = initializeSample(args) + + arguments + args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none' + args.H_offset (1,1) double {mustBeNumeric} = 0 % Vertical offset [m] + args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-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_offset = args.H_offset; + + sample.H = args.H; + sample.R = args.R; + sample.m = args.m; + end +end diff --git a/matlab/src/initializeStewartPlatform.m b/matlab/src/initializeStewartPlatform.m new file mode 100644 index 0000000..0545b4d --- /dev/null +++ b/matlab/src/initializeStewartPlatform.m @@ -0,0 +1,31 @@ +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(); diff --git a/matlab/src/initializeStewartPose.m b/matlab/src/initializeStewartPose.m new file mode 100644 index 0000000..325b501 --- /dev/null +++ b/matlab/src/initializeStewartPose.m @@ -0,0 +1,27 @@ +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; diff --git a/matlab/src/initializeStrutDynamics.m b/matlab/src/initializeStrutDynamics.m new file mode 100644 index 0000000..79c2898 --- /dev/null +++ b/matlab/src/initializeStrutDynamics.m @@ -0,0 +1,60 @@ +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 diff --git a/matlab/src/inverseKinematics.m b/matlab/src/inverseKinematics.m new file mode 100644 index 0000000..c1c0618 --- /dev/null +++ b/matlab/src/inverseKinematics.m @@ -0,0 +1,36 @@ +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; diff --git a/matlab/src/plotCube.m b/matlab/src/plotCube.m new file mode 100644 index 0000000..7367775 --- /dev/null +++ b/matlab/src/plotCube.m @@ -0,0 +1,58 @@ +function [] = plotCube(stewart, args) + +arguments + stewart + args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 + args.FOc (1,1) double {mustBeNumeric} = 50e-3 + args.color (4,1) double {mustBeNumeric} = [0,0,0,0.5] + args.linewidth (1,1) double {mustBeNumeric, mustBePositive} = 2.5 + args.link_to_struts logical {mustBeNumericOrLogical} = false +end + +sx = [ 2; -1; -1]; +sy = [ 0; 1; -1]; +sz = [ 1; 1; 1]; + +R = [sx, sy, sz]./vecnorm([sx, sy, sz]); + +L = args.Hc*sqrt(3); + +p_xyz = R'*[[0;0;0],[L;0;0],[L;L;0],[0;L;0],[0;0;L],[L;0;L],[L;L;L],[0;L;L]] - [0;0;1.5*args.Hc]; + +% Position center of the cube +p_xyz = p_xyz + args.FOc*[0;0;1]*ones(1,8); + +edges_order = [1 2 3 4 1]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [5 6 7 8 5]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [1 5]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [2 6]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [3 7]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); +edges_order = [4 8]; +plot3(p_xyz(1,edges_order), p_xyz(2,edges_order), p_xyz(3,edges_order), '-', 'color', args.color, 'linewidth', args.linewidth); + +if args.link_to_struts + Fb = stewart.platform_M.Mb + stewart.geometry.FO_M; + plot3([Fb(1,1), p_xyz(1,5)],... + [Fb(2,1), p_xyz(2,5)],... + [Fb(3,1), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,2), p_xyz(1,2)],... + [Fb(2,2), p_xyz(2,2)],... + [Fb(3,2), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,3), p_xyz(1,2)],... + [Fb(2,3), p_xyz(2,2)],... + [Fb(3,3), p_xyz(3,2)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,4), p_xyz(1,4)],... + [Fb(2,4), p_xyz(2,4)],... + [Fb(3,4), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,5), p_xyz(1,4)],... + [Fb(2,5), p_xyz(2,4)],... + [Fb(3,5), p_xyz(3,4)], '--', 'color', args.color, 'linewidth', args.linewidth); + plot3([Fb(1,6), p_xyz(1,5)],... + [Fb(2,6), p_xyz(2,5)],... + [Fb(3,6), p_xyz(3,5)], '--', 'color', args.color, 'linewidth', args.linewidth); +end diff --git a/matlab/src/plotCylindricalPayload.m b/matlab/src/plotCylindricalPayload.m new file mode 100644 index 0000000..93cb975 --- /dev/null +++ b/matlab/src/plotCylindricalPayload.m @@ -0,0 +1,15 @@ +function [] = plotCylindricalPayload(stewart, args) + +arguments + stewart + args.H (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + args.R (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.H_offset (1,1) double {mustBeNumeric} = 0 + args.color (3,1) double {mustBeNumeric} = [0.5,0.5,0.5] +end + +[X,Y,Z] = cylinder(args.R); +Z = args.H*Z + args.H_offset; +surf(X, Y, Z, 'facecolor', args.color, 'edgecolor', 'none') +fill3(X(1,:), Y(1,:), Z(1,:), 'k', 'facecolor', args.color) +fill3(X(2,:), Y(2,:), Z(2,:), 'k', 'facecolor', args.color) diff --git a/matlab/subsystems/metrology_6dof.slx b/matlab/subsystems/metrology_6dof.slx new file mode 100644 index 0000000..d6bc758 Binary files /dev/null and b/matlab/subsystems/metrology_6dof.slx differ diff --git a/matlab/subsystems/nano_hexapod_fixed_base.slx b/matlab/subsystems/nano_hexapod_fixed_base.slx new file mode 100644 index 0000000..9b7075a Binary files /dev/null and b/matlab/subsystems/nano_hexapod_fixed_base.slx differ diff --git a/matlab/subsystems/nano_hexapod_mobile_platform.slx b/matlab/subsystems/nano_hexapod_mobile_platform.slx new file mode 100644 index 0000000..78ef458 Binary files /dev/null and b/matlab/subsystems/nano_hexapod_mobile_platform.slx differ diff --git a/matlab/subsystems/nano_hexapod_simscape.slx b/matlab/subsystems/nano_hexapod_simscape.slx new file mode 100644 index 0000000..2c93e23 Binary files /dev/null and b/matlab/subsystems/nano_hexapod_simscape.slx differ diff --git a/matlab/subsystems/nano_hexapod_strut.slx b/matlab/subsystems/nano_hexapod_strut.slx new file mode 100644 index 0000000..78da33f Binary files /dev/null and b/matlab/subsystems/nano_hexapod_strut.slx differ diff --git a/paper/.latexmkrc b/paper/.latexmkrc new file mode 100644 index 0000000..edd570e --- /dev/null +++ b/paper/.latexmkrc @@ -0,0 +1,111 @@ +#!/bin/env perl + +# Shebang is only to get syntax highlighting right across GitLab, GitHub and IDEs. +# This file is not meant to be run, but read by `latexmk`. + +# ====================================================================================== +# Perl `latexmk` configuration file +# ====================================================================================== + +# ====================================================================================== +# PDF Generation/Building/Compilation +# ====================================================================================== + +@default_files=('dehaeze26_cubic_architecture.tex'); + +# PDF-generating modes are: +# 1: pdflatex, as specified by $pdflatex variable (still largely in use) +# 2: postscript conversion, as specified by the $ps2pdf variable (useless) +# 3: dvi conversion, as specified by the $dvipdf variable (useless) +# 4: lualatex, as specified by the $lualatex variable (best) +# 5: xelatex, as specified by the $xelatex variable (second best) +$pdf_mode = 1; + +# Treat undefined references and citations as well as multiply defined references as +# ERRORS instead of WARNINGS. +# This is only checked in the *last* run, since naturally, there are undefined references +# in initial runs. +# This setting is potentially annoying when debugging/editing, but highly desirable +# in the CI pipeline, where such a warning should result in a failed pipeline, since the +# final document is incomplete/corrupted. +# +# However, I could not eradicate all warnings, so that `latexmk` currently fails with +# this option enabled. +# Specifically, `microtype` fails together with `fontawesome`/`fontawesome5`, see: +# https://tex.stackexchange.com/a/547514/120853 +# The fix in that answer did not help. +# Setting `verbose=silent` to mute `microtype` warnings did not work. +# Switching between `fontawesome` and `fontawesome5` did not help. +$warnings_as_errors = 0; + +# Show used CPU time. Looks like: https://tex.stackexchange.com/a/312224/120853 +$show_time = 1; + +# Default is 5; we seem to need more owed to the complexity of the document. +# Actual documents probably don't need this many since they won't use all features, +# plus won't be compiling from cold each time. +$max_repeat=7; + +# --shell-escape option (execution of code outside of latex) is required for the +#'svg' package. +# It converts raw SVG files to the PDF+PDF_TEX combo using InkScape. +# +# SyncTeX allows to jump between source (code) and output (PDF) in IDEs with support +# (many have it). A value of `1` is enabled (gzipped), `-1` is enabled but uncompressed, +# `0` is off. +# Testing in VSCode w/ LaTeX Workshop only worked for the compressed version. +# Adjust this as needed. Of course, only relevant for local use, no effect on a remote +# CI pipeline (except for slower compilation, probably). +# +# %O and %S will forward Options and the Source file, respectively, given to latexmk. +# +# `set_tex_cmds` applies to all *latex commands (latex, xelatex, lualatex, ...), so +# no need to specify these each. This allows to simply change `$pdf_mode` to get a +# different engine. Check if this works with `latexmk --commands`. +set_tex_cmds("--shell-escape -interaction=nonstopmode --synctex=1 %O %S"); + +# Use default pdf viewer +$pdf_previewer = 'zathura'; + +# option 2 is same as 1 (run biber when necessary), but also deletes the +# regeneratable bbl-file in a clenaup (`latexmk -c`). Do not use if original +# bib file is not available! +$bibtex_use = 2; # default: 1 + +# Change default `biber` call, help catch errors faster/clearer. See +# https://web.archive.org/web/20200526101657/https://www.semipol.de/2018/06/12/latex-best-practices.html#database-entries +$biber = "biber --validate-datamodel %O %S"; + +# Glossaries +add_cus_dep('glo', 'gls', 0, 'run_makeglossaries'); +add_cus_dep('acn', 'acr', 0, 'run_makeglossaries'); + +sub run_makeglossaries { + if ( $silent ) { + system "makeglossaries -q -s '$_[0].ist' '$_[0]'"; + } + else { + system "makeglossaries -s '$_[0].ist' '$_[0]'"; + }; +} + +# ====================================================================================== +# Auxiliary Files +# ====================================================================================== + +# Let latexmk know about generated files, so they can be used to detect if a +# rerun is required, or be deleted in a cleanup. +# loe: List of Examples (KOMAScript) +# lol: List of Listings (`listings` and `minted` packages) +# run.xml: biber runs +# glg: glossaries log +# glstex: generated from glossaries-extra +push @generated_exts, 'loe', 'lol', 'run.xml', 'glstex', 'glo', 'gls', 'glg', 'acn', 'acr', 'alg'; + +# Also delete the *.glstex files from package glossaries-extra. Problem is, +# that that package generates files of the form "basename-digit.glstex" if +# multiple glossaries are present. Latexmk looks for "basename.glstex" and so +# does not find those. For that purpose, use wildcard. +# Also delete files generated by gnuplot/pgfplots contour plots +# (.dat, .script, .table). +$clean_ext = "%R-*.glstex %R_contourtmp*.*"; diff --git a/paper/dehaeze26_cubic_architecture.bib b/paper/dehaeze26_cubic_architecture.bib new file mode 100644 index 0000000..edd2102 --- /dev/null +++ b/paper/dehaeze26_cubic_architecture.bib @@ -0,0 +1,179 @@ +@article{stewart65_platf_with_six_degrees_freed, + author = {Stewart, D.}, + title = {A Platform With Six Degrees of Freedom}, + journal = {Proceedings of the institution of mechanical engineers}, + volume = 180, + number = 1, + pages = {371--386}, + year = 1965, + publisher = {Sage Publications Sage UK: London, England}, +} + + + + +@article{geng94_six_degree_of_freed_activ, + author = {Geng, Z. J. and Haynes, L. S.}, + title = {Six Degree-Of-Freedom Active Vibration Control Using the + Stewart Platforms}, + journal = {IEEE Transactions on Control Systems Technology}, + volume = 2, + number = 1, + pages = {45--53}, + year = 1994, + doi = {10.1109/87.273110}, + url = {https://doi.org/10.1109/87.273110}, + keywords = {parallel robot, cubic configuration}, +} + + + +@article{preumont07_six_axis_singl_stage_activ, + author = {Preumont, A. and Horodinca, M. and Romanescu, I. and de + Marneffe, B. and Avraam, M. and Deraemaeker, A. and Bossens, F. and + Abu Hanieh, A.}, + title = {A Six-Axis Single-Stage Active Vibration Isolator Based on + Stewart Platform}, + journal = {Journal of Sound and Vibration}, + volume = 300, + number = {3-5}, + pages = {644--661}, + year = 2007, + doi = {10.1016/j.jsv.2006.07.050}, + url = {https://doi.org/10.1016/j.jsv.2006.07.050}, + keywords = {parallel robot}, +} + + + +@article{jafari03_orthog_gough_stewar_platf_microm, + author = {Jafari, F. and McInroy, J. E.}, + title = {Orthogonal Gough-Stewart Platforms for Micromanipulation}, + journal = {IEEE Transactions on Robotics and Automation}, + volume = 19, + number = 4, + pages = {595--603}, + year = 2003, + doi = {10.1109/tra.2003.814506}, + url = {https://doi.org/10.1109/tra.2003.814506}, + issn = {1042-296X}, + keywords = {parallel robot, cubic configuration}, + month = 8, + publisher = {Institute of Electrical and Electronics Engineers (IEEE)}, +} + + + +@phdthesis{hanieh03_activ_stewar, + author = {Abu Hanieh, A.}, + keywords = {parallel robot}, + school = {Universit{\'e} Libre de Bruxelles, Brussels, Belgium}, + title = {Active isolation and damping of vibrations via Stewart + platform}, + year = 2003, +} + + + +@book{preumont18_vibrat_contr_activ_struc_fourt_edition, + author = {Preumont, A.}, + title = {Vibration Control of Active Structures - Fourth Edition}, + year = 2018, + publisher = {Springer International Publishing}, + url = {https://doi.org/10.1007/978-3-319-72296-2}, + doi = {10.1007/978-3-319-72296-2}, + keywords = {favorite, parallel robot}, + series = {Solid Mechanics and Its Applications}, +} + + + +@article{thayer02_six_axis_vibrat_isolat_system, + author = {Thayer, D. and Campbell, M. and Vagners, J. and + von Flotow, A.}, + title = {Six-Axis Vibration Isolation System Using Soft Actuators + and Multiple Sensors}, + journal = {Journal of Spacecraft and Rockets}, + volume = 39, + number = 2, + pages = {206--212}, + year = 2002, + doi = {10.2514/2.3821}, + url = {https://doi.org/10.2514/2.3821}, + keywords = {parallel robot}, +} + + + +@article{mcinroy00_desig_contr_flexur_joint_hexap, + author = {McInroy, J. E. and Hamann, J. C.}, + title = {Design and Control of Flexure Jointed Hexapods}, + journal = {IEEE Transactions on Robotics and Automation}, + volume = 16, + number = 4, + pages = {372--381}, + year = 2000, + doi = {10.1109/70.864229}, + url = {https://doi.org/10.1109/70.864229}, + keywords = {parallel robot}, +} + + + +@phdthesis{li01_simul_fault_vibrat_isolat_point, + author = {Li, X.}, + keywords = {parallel robot}, + school = {University of Wyoming}, + title = {Simultaneous, Fault-tolerant Vibration Isolation and + Pointing Control of Flexure Jointed Hexapods}, + year = 2001, +} + + + +@inproceedings{mcinroy99_dynam, + author = {McInroy, J. E.}, + title = {Dynamic modeling of flexure jointed hexapods for control + purposes}, + booktitle = {Proceedings of the 1999 IEEE International Conference on + Control Applications (Cat. No.99CH36328)}, + year = 1999, + doi = {10.1109/cca.1999.806694}, + url = {https://doi.org/10.1109/cca.1999.806694}, + keywords = {parallel robot}, +} + + + +@article{furutani04_nanom_cuttin_machin_using_stewar, + author = {Furutani, K. and Suzuki, M. and Kudoh, R.}, + title = {Nanometre-Cutting Machine Using a Stewart-Platform Parallel + Mechanism}, + journal = {Measurement Science and Technology}, + volume = 15, + number = 2, + pages = {467--474}, + year = 2004, + doi = {10.1088/0957-0233/15/2/022}, + url = {https://doi.org/10.1088/0957-0233/15/2/022}, + keywords = {parallel robot, cubic configuration}, +} + + + +@article{yang19_dynam_model_decoup_contr_flexib, + author = {Yang, X. and Wu, H. and Chen, B. and Kang, S. and Cheng, S.}, + title = {Dynamic Modeling and Decoupled Control of a Flexible + Stewart Platform for Vibration Isolation}, + journal = {Journal of Sound and Vibration}, + volume = 439, + pages = {398--412}, + year = 2019, + doi = {10.1016/j.jsv.2018.10.007}, + url = {https://doi.org/10.1016/j.jsv.2018.10.007}, + issn = {0022-460X}, + keywords = {parallel robot, flexure, decoupled control}, + month = 1, + publisher = {Elsevier BV}, +} + diff --git a/paper/dehaeze26_cubic_architecture.org b/paper/dehaeze26_cubic_architecture.org new file mode 100644 index 0000000..9b5223c --- /dev/null +++ b/paper/dehaeze26_cubic_architecture.org @@ -0,0 +1,567 @@ +#+TITLE: Decoupling Properties of the Cubic Architecture +:DRAWER: +#+LANGUAGE: en +#+EMAIL: dehaeze.thomas@gmail.com +#+AUTHOR: Dehaeze Thomas + +#+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{dehaeze26_cubic_architecture.bib} + +#+BIND: org-latex-bib-compiler "biber" +: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: +** Journal +https://asmedigitalcollection.asme.org/mechanicaldesign + +Guide: https://www.asme.org/publications-submissions/journals/information-for-authors/journal-guidelines/writing-a-research-paper + +#+begin_quote +Research papers undergo full peer review. Authors are encouraged to prepare concise manuscripts that convey clearly the significance of the work. Research Papers do not have a specified length but are usually 8,000 to 12,000 words with 5-8 figures or tables. +#+end_quote + +** TODO [#B] Add more content from the PhD thesis? + +Maybe add: +- [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/B1-nass-geometry/nass-geometry.org::*Review of Stewart platforms][Review of Stewart platforms]] +- [[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/B1-nass-geometry/nass-geometry.org::*Effect of geometry on Stewart platform properties][Effect of geometry on Stewart platform properties]] + +* Introduction :ignore: + +The Cubic configuration for the Stewart platform was first proposed by Dr. Gough in a comment to the original paper by Dr. Stewart [[cite:&stewart65_platf_with_six_degrees_freed]]. +This configuration is characterized by active struts arranged in a mutually orthogonal configuration connecting the corners of a cube, as shown in Figure ref:fig:detail_kinematics_cubic_architecture_example. + +Typically, the struts have similar length to the cube's edges, as illustrated in Figure ref:fig:detail_kinematics_cubic_architecture_example. +Practical implementations of such configurations can be observed in Figures ref:fig:detail_kinematics_jpl, ref:fig:detail_kinematics_uw_gsp and ref:fig:detail_kinematics_uqp. +It is also possible to implement designs with strut lengths smaller than the cube's edges (Figure ref:fig:detail_kinematics_cubic_architecture_example_small), as exemplified in Figure ref:fig:detail_kinematics_ulb_pz. + +#+name: fig:detail_kinematics_cubic_architecture_examples +#+caption: Typical Stewart platform cubic architectures in which struts' length is similar to the cube edges's length (\subref{fig:detail_kinematics_cubic_architecture_example}) or is taking just a portion of the edge (\subref{fig:detail_kinematics_cubic_architecture_example_small}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example}Classical Cubic architecture} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_architecture_example.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_architecture_example_small}Alternative configuration} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_architecture_example_small.png]] +#+end_subfigure +#+end_figure + + +Several advantageous properties attributed to the cubic configuration have contributed to its widespread adoption [[cite:&geng94_six_degree_of_freed_activ;&preumont07_six_axis_singl_stage_activ;&jafari03_orthog_gough_stewar_platf_microm]]: simplified kinematics relationships and dynamical analysis [[cite:&geng94_six_degree_of_freed_activ]]; uniform stiffness in all directions [[cite:&hanieh03_activ_stewar]]; uniform mobility [[cite:&preumont18_vibrat_contr_activ_struc_fourt_edition, chapt.8.5.2]]; and minimization of the cross coupling between actuators and sensors in different struts [[cite:&preumont07_six_axis_singl_stage_activ]]. +This minimization is attributed to the fact that the struts are orthogonal to each other, and is said to facilitate collocated sensor-actuator control system design, i.e., the implementation of decentralized control [[cite:&geng94_six_degree_of_freed_activ;&thayer02_six_axis_vibrat_isolat_system]]. + +These properties are examined in this section to assess their relevance for the nano-hexapod. +The mobility and stiffness properties of the cubic configuration are analyzed in Section ref:ssec:detail_kinematics_cubic_static. +Dynamical decoupling is investigated in Section ref:ssec:detail_kinematics_cubic_dynamic, while decentralized control, crucial for the NASS, is examined in Section ref:ssec:detail_kinematics_decentralized_control. +Given that the cubic architecture imposes strict geometric constraints, alternative designs are proposed in Section ref:ssec:detail_kinematics_cubic_design. +The ultimate objective is to determine the suitability of the cubic architecture for the nano-hexapod. + +* Static Properties +<> +** Stiffness matrix for the Cubic architecture + +Consider the cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_schematic_full. +The unit vectors corresponding to the edges of the cube are described by equation eqref:eq:detail_kinematics_cubic_s. + +\begin{equation}\label{eq:detail_kinematics_cubic_s} + \hat{\bm{s}}_1 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_2 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_3 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_4 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_5 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_6 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} +\end{equation} + +#+name: fig:detail_kinematics_cubic_schematic_cases +#+caption: Cubic architecture. Struts are represented in blue. The cube's center by a black dot. The Struts can match the cube's edges (\subref{fig:detail_kinematics_cubic_schematic_full}) or just take a portion of the edge (\subref{fig:detail_kinematics_cubic_schematic}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic_full}Full cube} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 0.9 +[[file:figs/detail_kinematics_cubic_schematic_full.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_schematic}Cube's portion} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 0.9 +[[file:figs/detail_kinematics_cubic_schematic.png]] +#+end_subfigure +#+end_figure + +Coordinates of the cube's vertices relevant for the top joints, expressed with respect to the cube's center, are shown in equation eqref:eq:detail_kinematics_cubic_vertices. + +\begin{equation}\label{eq:detail_kinematics_cubic_vertices} + \tilde{\bm{b}}_1 = \tilde{\bm{b}}_2 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{-\sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_3 = \tilde{\bm{b}}_4 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{ \sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_5 = \tilde{\bm{b}}_6 = H_c \begin{bmatrix} \frac{-2}{\sqrt{2}} \\ 0 \\ \frac{1}{2} \end{bmatrix} +\end{equation} + +In the case where top joints are positioned at the cube's vertices, a diagonal stiffness matrix is obtained as shown in equation eqref:eq:detail_kinematics_cubic_stiffness. +Translation stiffness is twice the stiffness of the struts, and rotational stiffness is proportional to the square of the cube's size $H_c$. + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness} + \bm{K}_{\{B\} = \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 2 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 & 0 \\ + 0 & 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +However, typically, the top joints are not placed at the cube's vertices but at positions along the cube's edges (Figure ref:fig:detail_kinematics_cubic_schematic). +In that case, the location of the top joints can be expressed by equation eqref:eq:detail_kinematics_cubic_edges, yet the computed stiffness matrix remains identical to Equation eqref:eq:detail_kinematics_cubic_stiffness. + +\begin{equation}\label{eq:detail_kinematics_cubic_edges} + \bm{b}_i = \tilde{\bm{b}}_i + \alpha \hat{\bm{s}}_i +\end{equation} + + +The stiffness matrix is therefore diagonal when the considered $\{B\}$ frame is located at the center of the cube (shown by frame $\{C\}$). +This means that static forces (resp torques) applied at the cube's center will induce pure translations (resp rotations around the cube's center). +This specific location where the stiffness matrix is diagonal is referred to as the "Center of Stiffness" (analogous to the "Center of Mass" where the mass matrix is diagonal). + +** Effect of having frame $\{B\}$ off-centered + +When the reference frames $\{A\}$ and $\{B\}$ are shifted from the cube's center, off-diagonal elements emerge in the stiffness matrix. + +Considering a vertical shift as shown in Figure ref:fig:detail_kinematics_cubic_schematic, the stiffness matrix transforms into that shown in Equation eqref:eq:detail_kinematics_cubic_stiffness_off_centered. +Off-diagonal elements increase proportionally with the height difference between the cube's center and the considered $\{B\}$ frame. + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness_off_centered} + \bm{K}_{\{B\} \neq \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & -2 H & 0 \\ + 0 & 2 & 0 & 2 H & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 2 H & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 & 0 \\ + -2 H & 0 & 0 & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +This stiffness matrix structure is characteristic of Stewart platforms exhibiting symmetry, and is not an exclusive property of cubic architectures. +Therefore, the stiffness characteristics of the cubic architecture are only distinctive when considering a reference frame located at the cube's center. +This poses a practical limitation, as in most applications, the relevant frame (where motion is of interest and forces are applied) is located above the top platform. + +It should be noted that for the stiffness matrix to be diagonal, the cube's center doesn't need to coincide with the geometric center of the Stewart platform. +This observation leads to the interesting alternative architectures presented in Section ref:ssec:detail_kinematics_cubic_design. + +** Uniform Mobility + +The translational mobility of the Stewart platform with constant orientation was analyzed. +Considering limited actuator stroke (elongation of each strut), the maximum achievable positions in XYZ space were estimated. +The resulting mobility in X, Y, and Z directions for the cubic architecture is illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_translations. + +The translational workspace analysis reveals that for the cubic architecture, the achievable positions form a cube whose axes align with the struts, with the cube's edge length corresponding to the strut axial stroke. +These findings suggest that the mobility pattern is more subtle than sometimes described in the literature [[cite:&mcinroy00_desig_contr_flexur_joint_hexap]], exhibiting uniformity primarily along directions aligned with the cube's edges rather than uniform spherical distribution in all XYZ directions. +This configuration still offers more consistent mobility characteristics compared to alternative architectures illustrated in Figure ref:fig:detail_kinematics_mobility_trans. + +The rotational mobility, illustrated in Figure ref:fig:detail_kinematics_cubic_mobility_rotations, exhibits greater achievable angular stroke in the $R_x$ and $R_y$ directions compared to the $R_z$ direction. +Furthermore, an inverse relationship exists between the cube's dimension and rotational mobility, with larger cube sizes corresponding to more limited angular displacement capabilities. + +#+name: fig:detail_kinematics_cubic_mobility +#+caption: Mobility of a Stewart platform with Cubic architecture. Both for translations (\subref{fig:detail_kinematics_cubic_mobility_translations}) and rotations (\subref{fig:detail_kinematics_cubic_mobility_rotations}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_translations}Mobility in translation} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_mobility_translations.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_mobility_rotations}Mobility in rotation} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :scale 1 +[[file:figs/detail_kinematics_cubic_mobility_rotations.png]] +#+end_subfigure +#+end_figure + +* Dynamical Decoupling +<> +** Introduction :ignore: + +This section examines the dynamics of the cubic architecture in the Cartesian frame which corresponds to the transfer function from forces and torques $\bm{\mathcal{F}}$ to translations and rotations $\bm{\mathcal{X}}$ of the top platform. +When relative motion sensors are integrated in each strut (measuring $\bm{\mathcal{L}}$), the pose $\bm{\mathcal{X}}$ is computed using the Jacobian matrix as shown in Figure ref:fig:detail_kinematics_centralized_control. + +#+name: fig:detail_kinematics_centralized_control +#+caption: Typical control architecture in the cartesian frame +[[file:figs/detail_kinematics_centralized_control.png]] + +** Low frequency and High frequency coupling + +As derived during the conceptual design phase, the dynamics from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ is described by Equation eqref:eq:detail_kinematics_transfer_function_cart. +At low frequency, the behavior of the platform depends on the stiffness matrix eqref:eq:detail_kinematics_transfer_function_cart_low_freq. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_cart_low_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to 0]{} \bm{K}^{-1} +\end{equation} + +In Section ref:ssec:detail_kinematics_cubic_static, it was demonstrated that for the cubic configuration, the stiffness matrix is diagonal if frame $\{B\}$ is positioned at the cube's center. +In this case, the "Cartesian" plant is decoupled at low frequency. +At high frequency, the behavior is governed by the mass matrix (evaluated at frame $\{B\}$) eqref:eq:detail_kinematics_transfer_function_high_freq. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_high_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to \infty]{} - \omega^2 \bm{M}^{-1} +\end{equation} + +To achieve a diagonal mass matrix, the center of mass of the mobile components must coincide with the $\{B\}$ frame, and the principal axes of inertia must align with the axes of the $\{B\}$ frame. + +#+name: fig:detail_kinematics_cubic_payload +#+caption: Cubic stewart platform with top cylindrical payload +#+attr_latex: :width 0.6\linewidth +[[file:figs/detail_kinematics_cubic_payload.png]] + +To verify these properties, a cubic Stewart platform with a cylindrical payload was analyzed (Figure ref:fig:detail_kinematics_cubic_payload). +Transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ were computed for two specific locations of the $\{B\}$ frames. +When the $\{B\}$ frame was positioned at the center of mass, coupling at low frequency was observed due to the non-diagonal stiffness matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com). +Conversely, when positioned at the center of stiffness, coupling occurred at high frequency due to the non-diagonal mass matrix (Figure ref:fig:detail_kinematics_cubic_cart_coupling_cok). + +#+name: fig:detail_kinematics_cubic_cart_coupling +#+caption: Transfer functions for a Cubic Stewart platform expressed in the Cartesian frame. Two locations of the $\{B\}$ frame are considered: at the center of mass of the moving body (\subref{fig:detail_kinematics_cubic_cart_coupling_com}) and at the cube's center (\subref{fig:detail_kinematics_cubic_cart_coupling_cok}). +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com}$\{B\}$ at the center of mass} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_com.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_cok}$\{B\}$ at the cube's center} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_cok.png]] +#+end_subfigure +#+end_figure + +** Payload's CoM at the cube's center + +An effective strategy for improving dynamical performances involves aligning the cube's center (center of stiffness) with the center of mass of the moving components [[cite:&li01_simul_fault_vibrat_isolat_point]]. +This can be achieved by positioning the payload below the top platform, such that the center of mass of the moving body coincides with the cube's center (Figure ref:fig:detail_kinematics_cubic_centered_payload). +This approach was physically implemented in several studies [[cite:&mcinroy99_dynam;&jafari03_orthog_gough_stewar_platf_microm]], as shown in Figure ref:fig:detail_kinematics_uw_gsp. +The resulting dynamics are indeed well-decoupled (Figure ref:fig:detail_kinematics_cubic_cart_coupling_com_cok), taking advantage from diagonal stiffness and mass matrices. +The primary limitation of this approach is that, for many applications including the nano-hexapod, the payload must be positioned above the top platform. +If a design similar to Figure ref:fig:detail_kinematics_cubic_centered_payload were employed for the nano-hexapod, the X-ray beam would intersect with the struts during spindle rotation. + +#+name: fig:detail_kinematics_cubic_com_cok +#+caption: Cubic Stewart platform with payload at the cube's center (\subref{fig:detail_kinematics_cubic_centered_payload}). Obtained cartesian plant is fully decoupled (\subref{fig:detail_kinematics_cubic_cart_coupling_com_cok}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_centered_payload}Payload at the cube's center} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_centered_payload.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com_cok}Fully decoupled cartesian plant} +#+attr_latex: :options {0.49\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_cart_coupling_com_cok.png]] +#+end_subfigure +#+end_figure + +** Conclusion + +The analysis of dynamical properties of the cubic architecture yields several important conclusions. +Static decoupling, characterized by a diagonal stiffness matrix, is achieved when reference frames $\{A\}$ and $\{B\}$ are positioned at the cube's center. +Note that this property can also be obtained with non-cubic architectures that exhibit symmetrical strut arrangements. +Dynamic decoupling requires both static decoupling and coincidence of the mobile platform's center of mass with reference frame $\{B\}$. +While this configuration offers powerful control advantages, it requires positioning the payload at the cube's center, which is highly restrictive and often impractical. + +* Decentralized Control +<> +** Introduction :ignore: + +The orthogonal arrangement of struts in the cubic architecture suggests a potential minimization of inter-strut coupling, which could theoretically create favorable conditions for decentralized control. +Two sensor types integrated in the struts are considered: displacement sensors and force sensors. +The control architecture is illustrated in Figure ref:fig:detail_kinematics_decentralized_control, where $\bm{K}_{\mathcal{L}}$ represents a diagonal transfer function matrix. + +#+name: fig:detail_kinematics_decentralized_control +#+caption: Decentralized control in the frame of the struts. +[[file:figs/detail_kinematics_decentralized_control.png]] + +The obtained plant dynamics in the frame of the struts are compared for two Stewart platforms. +The first employs a cubic architecture shown in Figure ref:fig:detail_kinematics_cubic_payload. +The second uses a non-cubic Stewart platform shown in Figure ref:fig:detail_kinematics_non_cubic_payload, featuring identical payload and strut dynamics but with struts oriented more vertically to differentiate it from the cubic architecture. + +#+name: fig:detail_kinematics_non_cubic_payload +#+caption: Stewart platform with non-cubic architecture +#+attr_latex: :width 0.6\linewidth +[[file:figs/detail_kinematics_non_cubic_payload.png]] + +** Relative Displacement Sensors + +The transfer functions from actuator force in each strut to the relative motion of the struts are presented in Figure ref:fig:detail_kinematics_decentralized_dL. +As anticipated from the equations of motion from $\bm{f}$ to $\bm{\mathcal{L}}$ eqref:eq:detail_kinematics_transfer_function_struts, the $6 \times 6$ plant is decoupled at low frequency. +At high frequency, coupling is observed as the mass matrix projected in the strut frame is not diagonal. + +No significant advantage is evident for the cubic architecture (Figure ref:fig:detail_kinematics_cubic_decentralized_dL) compared to the non-cubic architecture (Figure ref:fig:detail_kinematics_non_cubic_decentralized_dL). +The resonance frequencies differ between the two cases because the more vertical strut orientation in the non-cubic architecture alters the stiffness properties of the Stewart platform, consequently shifting the frequencies of various modes. + +#+name: fig:detail_kinematics_decentralized_dL +#+caption: Bode plot of the transfer functions from actuator force to relative displacement sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_dL}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_dL}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_dL}Non cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_non_cubic_decentralized_dL.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_dL}Cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_decentralized_dL.png]] +#+end_subfigure +#+end_figure + +** Force Sensors + +Similarly, the transfer functions from actuator force to force sensors in each strut were analyzed for both cubic and non-cubic Stewart platforms. +The results are presented in Figure ref:fig:detail_kinematics_decentralized_fn. +The system demonstrates good decoupling at high frequency in both cases, with no clear advantage for the cubic architecture. + +#+name: fig:detail_kinematics_decentralized_fn +#+caption: Bode plot of the transfer functions from actuator force to force sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_fn}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_fn}) +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_fn}Non cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_non_cubic_decentralized_fn.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_decentralized_fn}Cubic architecture} +#+attr_latex: :options {0.48\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.95\linewidth +[[file:figs/detail_kinematics_cubic_decentralized_fn.png]] +#+end_subfigure +#+end_figure + +** Conclusion + +The presented results do not demonstrate the pronounced decoupling advantages often associated with cubic architectures in the literature. +Both the cubic and non-cubic configurations exhibited similar coupling characteristics, suggesting that the benefits of orthogonal strut arrangement for decentralized control is less obvious than often reported in the literature. + +* Cubic architecture with Cube's center above the top platform +<> +** Introduction :ignore: + +As demonstrated in Section ref:ssec:detail_kinematics_cubic_dynamic, the cubic architecture can exhibit advantageous dynamical properties when the center of mass of the moving body coincides with the cube's center, resulting in diagonal mass and stiffness matrices. +As shown in Section ref:ssec:detail_kinematics_cubic_static, the stiffness matrix is diagonal when the considered $\{B\}$ frame is located at the cube's center. +However, the $\{B\}$ frame is typically positioned above the top platform where forces are applied and displacements are measured. + +This section proposes modifications to the cubic architecture to enable positioning the payload above the top platform while still leveraging the advantageous dynamical properties of the cubic configuration. + +Three key parameters define the geometry of the cubic Stewart platform: $H$, the height of the Stewart platform (distance from fixed base to mobile platform); $H_c$, the height of the cube, as shown in Figure ref:fig:detail_kinematics_cubic_schematic_full; and $H_{CoM}$, the height of the center of mass relative to the mobile platform (coincident with the cube's center). + +Depending on the cube's size $H_c$ in relation to $H$ and $H_{CoM}$, different designs emerge. +In the following examples, $H = 100\,mm$ and $H_{CoM} = 20\,mm$. + +** Small cube + +When the cube size $H_c$ is smaller than twice the height of the CoM $H_{CoM}$ eqref:eq:detail_kinematics_cube_small, the resulting design is shown in Figure ref:fig:detail_kinematics_cubic_above_small. + +\begin{equation}\label{eq:detail_kinematics_cube_small} + H_c < 2 H_{CoM} +\end{equation} + +# TODO - Add link to Figure ref:fig:nhexa_stewart_piezo_furutani (page pageref:fig:nhexa_stewart_piezo_furutani) +This configuration is similar to that described in [[cite:&furutani04_nanom_cuttin_machin_using_stewar]], although they do not explicitly identify it as a cubic configuration. +Adjacent struts are parallel to each other, differing from the typical architecture where parallel struts are positioned opposite to each other. + +This approach yields a compact architecture, but the small cube size may result in insufficient rotational stiffness. + +#+name: fig:detail_kinematics_cubic_above_small +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 40mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_small_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_small_top.png]] +#+end_subfigure +#+end_figure + +** Medium sized cube + +Increasing the cube's size such that eqref:eq:detail_kinematics_cube_medium is verified produces an architecture with intersecting struts (Figure ref:fig:detail_kinematics_cubic_above_medium). + +\begin{equation}\label{eq:detail_kinematics_cube_medium} + 2 H_{CoM} < H_c < 2 (H_{CoM} + H) +\end{equation} + +This configuration resembles the design proposed in [[cite:&yang19_dynam_model_decoup_contr_flexib]] (Figure ref:fig:detail_kinematics_yang19), although their design is not strictly cubic. + +#+name: fig:detail_kinematics_cubic_above_medium +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 140mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_medium_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_medium_top.png]] +#+end_subfigure +#+end_figure + +** Large cube + +When the cube's height exceeds twice the sum of the platform height and CoM height eqref:eq:detail_kinematics_cube_large, the architecture shown in Figure ref:fig:detail_kinematics_cubic_above_large is obtained. + +\begin{equation}\label{eq:detail_kinematics_cube_large} + 2 (H_{CoM} + H) < H_c +\end{equation} + +#+name: fig:detail_kinematics_cubic_above_large +#+caption: Cubic architecture with cube's center above the top platform. A cube height of 240mm is used. +#+attr_latex: :options [htbp] +#+begin_figure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_iso}Isometric view} +#+attr_latex: :options {0.36\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_iso.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_side}Side view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_side.png]] +#+end_subfigure +#+attr_latex: :caption \subcaption{\label{fig:detail_kinematics_cubic_above_large_top}Top view} +#+attr_latex: :options {0.30\textwidth} +#+begin_subfigure +#+attr_latex: :width 0.9\linewidth +[[file:figs/detail_kinematics_cubic_above_large_top.png]] +#+end_subfigure +#+end_figure + +** Platform size + +For the proposed configuration, the top joints $\bm{b}_i$ (resp. the bottom joints $\bm{a}_i$) and are positioned on a circle with radius $R_{b_i}$ (resp. $R_{a_i}$) described by Equation eqref:eq:detail_kinematics_cube_joints. + +\begin{subequations}\label{eq:detail_kinematics_cube_joints} +\begin{align} + R_{b_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 H_{CoM}^2} \label{eq:detail_kinematics_cube_top_joints} \\ + R_{a_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 (H_{CoM} + H)^2} \label{eq:detail_kinematics_cube_bot_joints} +\end{align} +\end{subequations} + +Since the rotational stiffness for the cubic architecture scales with the square of the cube's height eqref:eq:detail_kinematics_cubic_stiffness, the cube's size can be determined based on rotational stiffness requirements. +Subsequently, using Equation eqref:eq:detail_kinematics_cube_joints, the dimensions of the top and bottom platforms can be calculated. + +* Conclusion +:PROPERTIES: +:UNNUMBERED: t +:END: + +The analysis of the cubic architecture for Stewart platforms yielded several important findings. +While the cubic configuration provides uniform stiffness in the XYZ directions, it stiffness property becomes particularly advantageous when forces and torques are applied at the cube's center. +Under these conditions, the stiffness matrix becomes diagonal, resulting in a decoupled Cartesian plant at low frequencies. + +Regarding mobility, the translational capabilities of the cubic configuration exhibit uniformity along the directions of the orthogonal struts, rather than complete uniformity in the Cartesian space. +This understanding refines the characterization of cubic architecture mobility commonly presented in literature. + +The analysis of decentralized control in the frame of the struts revealed more nuanced results than expected. +While cubic architectures are frequently associated with reduced coupling between actuators and sensors, this study showed that these benefits may be more subtle or context-dependent than commonly described. +Under the conditions analyzed, the coupling characteristics of cubic and non-cubic configurations, in the frame of the struts, appeared similar. + +Fully decoupled dynamics in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center. +However, this arrangement presents practical challenges, as the cube's center is traditionally located between the top and bottom platforms, making payload placement problematic for many applications. + +To address this limitation, modified cubic architectures have been proposed with the cube's center positioned above the top platform. +Three distinct configurations have been identified, each with different geometric arrangements but sharing the common characteristic that the cube's center is positioned above the top platform. +This structural modification enables the alignment of the moving body's center of mass with the center of stiffness, resulting in beneficial decoupling properties in the Cartesian frame. + +* Bibliography :ignore: +#+latex: \printbibliography[heading=bibintoc,title={Bibliography}] diff --git a/paper/dehaeze26_cubic_architecture.pdf b/paper/dehaeze26_cubic_architecture.pdf new file mode 100644 index 0000000..8fae0db Binary files /dev/null and b/paper/dehaeze26_cubic_architecture.pdf differ diff --git a/paper/dehaeze26_cubic_architecture.tex b/paper/dehaeze26_cubic_architecture.tex new file mode 100644 index 0000000..e28c2e4 --- /dev/null +++ b/paper/dehaeze26_cubic_architecture.tex @@ -0,0 +1,469 @@ +% Created 2025-11-26 Wed 09:39 +% Intended LaTeX compiler: pdflatex +\documentclass[a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]{scrreprt} + +\input{preamble.tex} +\input{preamble_extra.tex} +\bibliography{dehaeze26_cubic_architecture.bib} +\author{Dehaeze Thomas} +\date{\today} +\title{Decoupling Properties of the Cubic Architecture} +\hypersetup{ + pdfauthor={Dehaeze Thomas}, + pdftitle={Decoupling Properties of the Cubic Architecture}, + pdfkeywords={}, + pdfsubject={}, + pdfcreator={Emacs 30.2 (Org mode 9.7.34)}, + pdflang={English}} +\usepackage{biblatex} + +\begin{document} + +\maketitle +\tableofcontents + +\clearpage +The Cubic configuration for the Stewart platform was first proposed by Dr. Gough in a comment to the original paper by Dr. Stewart \cite{stewart65_platf_with_six_degrees_freed}. +This configuration is characterized by active struts arranged in a mutually orthogonal configuration connecting the corners of a cube, as shown in Figure \ref{fig:detail_kinematics_cubic_architecture_example}. + +Typically, the struts have similar length to the cube's edges, as illustrated in Figure \ref{fig:detail_kinematics_cubic_architecture_example}. +Practical implementations of such configurations can be observed in Figures \ref{fig:detail_kinematics_jpl}, \ref{fig:detail_kinematics_uw_gsp} and \ref{fig:detail_kinematics_uqp}. +It is also possible to implement designs with strut lengths smaller than the cube's edges (Figure \ref{fig:detail_kinematics_cubic_architecture_example_small}), as exemplified in Figure \ref{fig:detail_kinematics_ulb_pz}. + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/detail_kinematics_cubic_architecture_example.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_architecture_example}Classical Cubic architecture} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/detail_kinematics_cubic_architecture_example_small.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_architecture_example_small}Alternative configuration} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_architecture_examples}Typical Stewart platform cubic architectures in which struts' length is similar to the cube edges's length (\subref{fig:detail_kinematics_cubic_architecture_example}) or is taking just a portion of the edge (\subref{fig:detail_kinematics_cubic_architecture_example_small}).} +\end{figure} + + +Several advantageous properties attributed to the cubic configuration have contributed to its widespread adoption \cite{geng94_six_degree_of_freed_activ,preumont07_six_axis_singl_stage_activ,jafari03_orthog_gough_stewar_platf_microm}: simplified kinematics relationships and dynamical analysis \cite{geng94_six_degree_of_freed_activ}; uniform stiffness in all directions \cite{hanieh03_activ_stewar}; uniform mobility \cite[, chapt.8.5.2]{preumont18_vibrat_contr_activ_struc_fourt_edition}; and minimization of the cross coupling between actuators and sensors in different struts \cite{preumont07_six_axis_singl_stage_activ}. +This minimization is attributed to the fact that the struts are orthogonal to each other, and is said to facilitate collocated sensor-actuator control system design, i.e., the implementation of decentralized control \cite{geng94_six_degree_of_freed_activ,thayer02_six_axis_vibrat_isolat_system}. + +These properties are examined in this section to assess their relevance for the nano-hexapod. +The mobility and stiffness properties of the cubic configuration are analyzed in Section \ref{ssec:detail_kinematics_cubic_static}. +Dynamical decoupling is investigated in Section \ref{ssec:detail_kinematics_cubic_dynamic}, while decentralized control, crucial for the NASS, is examined in Section \ref{ssec:detail_kinematics_decentralized_control}. +Given that the cubic architecture imposes strict geometric constraints, alternative designs are proposed in Section \ref{ssec:detail_kinematics_cubic_design}. +The ultimate objective is to determine the suitability of the cubic architecture for the nano-hexapod. +\chapter{Static Properties} +\label{ssec:detail_kinematics_cubic_static} +\section{Stiffness matrix for the Cubic architecture} + +Consider the cubic architecture shown in Figure \ref{fig:detail_kinematics_cubic_schematic_full}. +The unit vectors corresponding to the edges of the cube are described by equation \eqref{eq:detail_kinematics_cubic_s}. + +\begin{equation}\label{eq:detail_kinematics_cubic_s} + \hat{\bm{s}}_1 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_2 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_3 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_4 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_5 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad + \hat{\bm{s}}_6 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{ 1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} +\end{equation} + +\begin{figure}[htbp] +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,scale=0.9]{figs/detail_kinematics_cubic_schematic_full.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_schematic_full}Full cube} +\end{subfigure} +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,scale=0.9]{figs/detail_kinematics_cubic_schematic.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_schematic}Cube's portion} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_schematic_cases}Cubic architecture. Struts are represented in blue. The cube's center by a black dot. The Struts can match the cube's edges (\subref{fig:detail_kinematics_cubic_schematic_full}) or just take a portion of the edge (\subref{fig:detail_kinematics_cubic_schematic})} +\end{figure} + +Coordinates of the cube's vertices relevant for the top joints, expressed with respect to the cube's center, are shown in equation \eqref{eq:detail_kinematics_cubic_vertices}. + +\begin{equation}\label{eq:detail_kinematics_cubic_vertices} + \tilde{\bm{b}}_1 = \tilde{\bm{b}}_2 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{-\sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_3 = \tilde{\bm{b}}_4 = H_c \begin{bmatrix} \frac{1}{\sqrt{2}} \\ \frac{ \sqrt{3}}{\sqrt{2}} \\ \frac{1}{2} \end{bmatrix}, \quad + \tilde{\bm{b}}_5 = \tilde{\bm{b}}_6 = H_c \begin{bmatrix} \frac{-2}{\sqrt{2}} \\ 0 \\ \frac{1}{2} \end{bmatrix} +\end{equation} + +In the case where top joints are positioned at the cube's vertices, a diagonal stiffness matrix is obtained as shown in equation \eqref{eq:detail_kinematics_cubic_stiffness}. +Translation stiffness is twice the stiffness of the struts, and rotational stiffness is proportional to the square of the cube's size \(H_c\). + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness} + \bm{K}_{\{B\} = \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & 0 & 0 \\ + 0 & 2 & 0 & 0 & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 & 0 \\ + 0 & 0 & 0 & 0 & \frac{3}{2} H_c^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +However, typically, the top joints are not placed at the cube's vertices but at positions along the cube's edges (Figure \ref{fig:detail_kinematics_cubic_schematic}). +In that case, the location of the top joints can be expressed by equation \eqref{eq:detail_kinematics_cubic_edges}, yet the computed stiffness matrix remains identical to Equation \eqref{eq:detail_kinematics_cubic_stiffness}. + +\begin{equation}\label{eq:detail_kinematics_cubic_edges} + \bm{b}_i = \tilde{\bm{b}}_i + \alpha \hat{\bm{s}}_i +\end{equation} + + +The stiffness matrix is therefore diagonal when the considered \(\{B\}\) frame is located at the center of the cube (shown by frame \(\{C\}\)). +This means that static forces (resp torques) applied at the cube's center will induce pure translations (resp rotations around the cube's center). +This specific location where the stiffness matrix is diagonal is referred to as the ``Center of Stiffness'' (analogous to the ``Center of Mass'' where the mass matrix is diagonal). +\section{Effect of having frame \(\{B\}\) off-centered} + +When the reference frames \(\{A\}\) and \(\{B\}\) are shifted from the cube's center, off-diagonal elements emerge in the stiffness matrix. + +Considering a vertical shift as shown in Figure \ref{fig:detail_kinematics_cubic_schematic}, the stiffness matrix transforms into that shown in Equation \eqref{eq:detail_kinematics_cubic_stiffness_off_centered}. +Off-diagonal elements increase proportionally with the height difference between the cube's center and the considered \(\{B\}\) frame. + +\begin{equation}\label{eq:detail_kinematics_cubic_stiffness_off_centered} + \bm{K}_{\{B\} \neq \{C\}} = k \begin{bmatrix} + 2 & 0 & 0 & 0 & -2 H & 0 \\ + 0 & 2 & 0 & 2 H & 0 & 0 \\ + 0 & 0 & 2 & 0 & 0 & 0 \\ + 0 & 2 H & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 & 0 \\ + -2 H & 0 & 0 & 0 & \frac{3}{2} H_c^2 + 2 H^2 & 0 \\ + 0 & 0 & 0 & 0 & 0 & 6 H_c^2 \\ + \end{bmatrix} +\end{equation} + +This stiffness matrix structure is characteristic of Stewart platforms exhibiting symmetry, and is not an exclusive property of cubic architectures. +Therefore, the stiffness characteristics of the cubic architecture are only distinctive when considering a reference frame located at the cube's center. +This poses a practical limitation, as in most applications, the relevant frame (where motion is of interest and forces are applied) is located above the top platform. + +It should be noted that for the stiffness matrix to be diagonal, the cube's center doesn't need to coincide with the geometric center of the Stewart platform. +This observation leads to the interesting alternative architectures presented in Section \ref{ssec:detail_kinematics_cubic_design}. +\section{Uniform Mobility} + +The translational mobility of the Stewart platform with constant orientation was analyzed. +Considering limited actuator stroke (elongation of each strut), the maximum achievable positions in XYZ space were estimated. +The resulting mobility in X, Y, and Z directions for the cubic architecture is illustrated in Figure \ref{fig:detail_kinematics_cubic_mobility_translations}. + +The translational workspace analysis reveals that for the cubic architecture, the achievable positions form a cube whose axes align with the struts, with the cube's edge length corresponding to the strut axial stroke. +These findings suggest that the mobility pattern is more subtle than sometimes described in the literature \cite{mcinroy00_desig_contr_flexur_joint_hexap}, exhibiting uniformity primarily along directions aligned with the cube's edges rather than uniform spherical distribution in all XYZ directions. +This configuration still offers more consistent mobility characteristics compared to alternative architectures illustrated in Figure \ref{fig:detail_kinematics_mobility_trans}. + +The rotational mobility, illustrated in Figure \ref{fig:detail_kinematics_cubic_mobility_rotations}, exhibits greater achievable angular stroke in the \(R_x\) and \(R_y\) directions compared to the \(R_z\) direction. +Furthermore, an inverse relationship exists between the cube's dimension and rotational mobility, with larger cube sizes corresponding to more limited angular displacement capabilities. + +\begin{figure}[htbp] +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/detail_kinematics_cubic_mobility_translations.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_mobility_translations}Mobility in translation} +\end{subfigure} +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,scale=1]{figs/detail_kinematics_cubic_mobility_rotations.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_mobility_rotations}Mobility in rotation} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_mobility}Mobility of a Stewart platform with Cubic architecture. Both for translations (\subref{fig:detail_kinematics_cubic_mobility_translations}) and rotations (\subref{fig:detail_kinematics_cubic_mobility_rotations})} +\end{figure} +\chapter{Dynamical Decoupling} +\label{ssec:detail_kinematics_cubic_dynamic} +This section examines the dynamics of the cubic architecture in the Cartesian frame which corresponds to the transfer function from forces and torques \(\bm{\mathcal{F}}\) to translations and rotations \(\bm{\mathcal{X}}\) of the top platform. +When relative motion sensors are integrated in each strut (measuring \(\bm{\mathcal{L}}\)), the pose \(\bm{\mathcal{X}}\) is computed using the Jacobian matrix as shown in Figure \ref{fig:detail_kinematics_centralized_control}. + +\begin{figure}[htbp] +\centering +\includegraphics[scale=1]{figs/detail_kinematics_centralized_control.png} +\caption{\label{fig:detail_kinematics_centralized_control}Typical control architecture in the cartesian frame} +\end{figure} +\section{Low frequency and High frequency coupling} + +As derived during the conceptual design phase, the dynamics from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{X}}\) is described by Equation \eqref{eq:detail_kinematics_transfer_function_cart}. +At low frequency, the behavior of the platform depends on the stiffness matrix \eqref{eq:detail_kinematics_transfer_function_cart_low_freq}. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_cart_low_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to 0]{} \bm{K}^{-1} +\end{equation} + +In Section \ref{ssec:detail_kinematics_cubic_static}, it was demonstrated that for the cubic configuration, the stiffness matrix is diagonal if frame \(\{B\}\) is positioned at the cube's center. +In this case, the ``Cartesian'' plant is decoupled at low frequency. +At high frequency, the behavior is governed by the mass matrix (evaluated at frame \(\{B\}\)) \eqref{eq:detail_kinematics_transfer_function_high_freq}. + +\begin{equation}\label{eq:detail_kinematics_transfer_function_high_freq} + \frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(j \omega) \xrightarrow[\omega \to \infty]{} - \omega^2 \bm{M}^{-1} +\end{equation} + +To achieve a diagonal mass matrix, the center of mass of the mobile components must coincide with the \(\{B\}\) frame, and the principal axes of inertia must align with the axes of the \(\{B\}\) frame. + +\begin{figure}[htbp] +\centering +\includegraphics[scale=1,width=0.6\linewidth]{figs/detail_kinematics_cubic_payload.png} +\caption{\label{fig:detail_kinematics_cubic_payload}Cubic stewart platform with top cylindrical payload} +\end{figure} + +To verify these properties, a cubic Stewart platform with a cylindrical payload was analyzed (Figure \ref{fig:detail_kinematics_cubic_payload}). +Transfer functions from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{X}}\) were computed for two specific locations of the \(\{B\}\) frames. +When the \(\{B\}\) frame was positioned at the center of mass, coupling at low frequency was observed due to the non-diagonal stiffness matrix (Figure \ref{fig:detail_kinematics_cubic_cart_coupling_com}). +Conversely, when positioned at the center of stiffness, coupling occurred at high frequency due to the non-diagonal mass matrix (Figure \ref{fig:detail_kinematics_cubic_cart_coupling_cok}). + +\begin{figure}[htbp] +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_cart_coupling_com.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com}$\{B\}$ at the center of mass} +\end{subfigure} +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_cart_coupling_cok.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_cok}$\{B\}$ at the cube's center} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_cart_coupling}Transfer functions for a Cubic Stewart platform expressed in the Cartesian frame. Two locations of the \(\{B\}\) frame are considered: at the center of mass of the moving body (\subref{fig:detail_kinematics_cubic_cart_coupling_com}) and at the cube's center (\subref{fig:detail_kinematics_cubic_cart_coupling_cok}).} +\end{figure} +\section{Payload's CoM at the cube's center} + +An effective strategy for improving dynamical performances involves aligning the cube's center (center of stiffness) with the center of mass of the moving components \cite{li01_simul_fault_vibrat_isolat_point}. +This can be achieved by positioning the payload below the top platform, such that the center of mass of the moving body coincides with the cube's center (Figure \ref{fig:detail_kinematics_cubic_centered_payload}). +This approach was physically implemented in several studies \cite{mcinroy99_dynam,jafari03_orthog_gough_stewar_platf_microm}, as shown in Figure \ref{fig:detail_kinematics_uw_gsp}. +The resulting dynamics are indeed well-decoupled (Figure \ref{fig:detail_kinematics_cubic_cart_coupling_com_cok}), taking advantage from diagonal stiffness and mass matrices. +The primary limitation of this approach is that, for many applications including the nano-hexapod, the payload must be positioned above the top platform. +If a design similar to Figure \ref{fig:detail_kinematics_cubic_centered_payload} were employed for the nano-hexapod, the X-ray beam would intersect with the struts during spindle rotation. + +\begin{figure}[htbp] +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_centered_payload.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_centered_payload}Payload at the cube's center} +\end{subfigure} +\begin{subfigure}{0.49\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_cart_coupling_com_cok.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_cart_coupling_com_cok}Fully decoupled cartesian plant} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_com_cok}Cubic Stewart platform with payload at the cube's center (\subref{fig:detail_kinematics_cubic_centered_payload}). Obtained cartesian plant is fully decoupled (\subref{fig:detail_kinematics_cubic_cart_coupling_com_cok})} +\end{figure} +\section{Conclusion} + +The analysis of dynamical properties of the cubic architecture yields several important conclusions. +Static decoupling, characterized by a diagonal stiffness matrix, is achieved when reference frames \(\{A\}\) and \(\{B\}\) are positioned at the cube's center. +Note that this property can also be obtained with non-cubic architectures that exhibit symmetrical strut arrangements. +Dynamic decoupling requires both static decoupling and coincidence of the mobile platform's center of mass with reference frame \(\{B\}\). +While this configuration offers powerful control advantages, it requires positioning the payload at the cube's center, which is highly restrictive and often impractical. +\chapter{Decentralized Control} +\label{ssec:detail_kinematics_decentralized_control} +The orthogonal arrangement of struts in the cubic architecture suggests a potential minimization of inter-strut coupling, which could theoretically create favorable conditions for decentralized control. +Two sensor types integrated in the struts are considered: displacement sensors and force sensors. +The control architecture is illustrated in Figure \ref{fig:detail_kinematics_decentralized_control}, where \(\bm{K}_{\mathcal{L}}\) represents a diagonal transfer function matrix. + +\begin{figure}[htbp] +\centering +\includegraphics[scale=1]{figs/detail_kinematics_decentralized_control.png} +\caption{\label{fig:detail_kinematics_decentralized_control}Decentralized control in the frame of the struts.} +\end{figure} + +The obtained plant dynamics in the frame of the struts are compared for two Stewart platforms. +The first employs a cubic architecture shown in Figure \ref{fig:detail_kinematics_cubic_payload}. +The second uses a non-cubic Stewart platform shown in Figure \ref{fig:detail_kinematics_non_cubic_payload}, featuring identical payload and strut dynamics but with struts oriented more vertically to differentiate it from the cubic architecture. + +\begin{figure}[htbp] +\centering +\includegraphics[scale=1,width=0.6\linewidth]{figs/detail_kinematics_non_cubic_payload.png} +\caption{\label{fig:detail_kinematics_non_cubic_payload}Stewart platform with non-cubic architecture} +\end{figure} +\section{Relative Displacement Sensors} + +The transfer functions from actuator force in each strut to the relative motion of the struts are presented in Figure \ref{fig:detail_kinematics_decentralized_dL}. +As anticipated from the equations of motion from \(\bm{f}\) to \(\bm{\mathcal{L}}\) \eqref{eq:detail_kinematics_transfer_function_struts}, the \(6 \times 6\) plant is decoupled at low frequency. +At high frequency, coupling is observed as the mass matrix projected in the strut frame is not diagonal. + +No significant advantage is evident for the cubic architecture (Figure \ref{fig:detail_kinematics_cubic_decentralized_dL}) compared to the non-cubic architecture (Figure \ref{fig:detail_kinematics_non_cubic_decentralized_dL}). +The resonance frequencies differ between the two cases because the more vertical strut orientation in the non-cubic architecture alters the stiffness properties of the Stewart platform, consequently shifting the frequencies of various modes. + +\begin{figure}[htbp] +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_non_cubic_decentralized_dL.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_dL}Non cubic architecture} +\end{subfigure} +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_decentralized_dL.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_decentralized_dL}Cubic architecture} +\end{subfigure} +\caption{\label{fig:detail_kinematics_decentralized_dL}Bode plot of the transfer functions from actuator force to relative displacement sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_dL}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_dL})} +\end{figure} +\section{Force Sensors} + +Similarly, the transfer functions from actuator force to force sensors in each strut were analyzed for both cubic and non-cubic Stewart platforms. +The results are presented in Figure \ref{fig:detail_kinematics_decentralized_fn}. +The system demonstrates good decoupling at high frequency in both cases, with no clear advantage for the cubic architecture. + +\begin{figure}[htbp] +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_non_cubic_decentralized_fn.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_non_cubic_decentralized_fn}Non cubic architecture} +\end{subfigure} +\begin{subfigure}{0.48\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.95\linewidth]{figs/detail_kinematics_cubic_decentralized_fn.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_decentralized_fn}Cubic architecture} +\end{subfigure} +\caption{\label{fig:detail_kinematics_decentralized_fn}Bode plot of the transfer functions from actuator force to force sensor in each strut. Both for a non-cubic architecture (\subref{fig:detail_kinematics_non_cubic_decentralized_fn}) and for a cubic architecture (\subref{fig:detail_kinematics_cubic_decentralized_fn})} +\end{figure} +\section{Conclusion} + +The presented results do not demonstrate the pronounced decoupling advantages often associated with cubic architectures in the literature. +Both the cubic and non-cubic configurations exhibited similar coupling characteristics, suggesting that the benefits of orthogonal strut arrangement for decentralized control is less obvious than often reported in the literature. +\chapter{Cubic architecture with Cube's center above the top platform} +\label{ssec:detail_kinematics_cubic_design} +As demonstrated in Section \ref{ssec:detail_kinematics_cubic_dynamic}, the cubic architecture can exhibit advantageous dynamical properties when the center of mass of the moving body coincides with the cube's center, resulting in diagonal mass and stiffness matrices. +As shown in Section \ref{ssec:detail_kinematics_cubic_static}, the stiffness matrix is diagonal when the considered \(\{B\}\) frame is located at the cube's center. +However, the \(\{B\}\) frame is typically positioned above the top platform where forces are applied and displacements are measured. + +This section proposes modifications to the cubic architecture to enable positioning the payload above the top platform while still leveraging the advantageous dynamical properties of the cubic configuration. + +Three key parameters define the geometry of the cubic Stewart platform: \(H\), the height of the Stewart platform (distance from fixed base to mobile platform); \(H_c\), the height of the cube, as shown in Figure \ref{fig:detail_kinematics_cubic_schematic_full}; and \(H_{CoM}\), the height of the center of mass relative to the mobile platform (coincident with the cube's center). + +Depending on the cube's size \(H_c\) in relation to \(H\) and \(H_{CoM}\), different designs emerge. +In the following examples, \(H = 100\,mm\) and \(H_{CoM} = 20\,mm\). +\section{Small cube} + +When the cube size \(H_c\) is smaller than twice the height of the CoM \(H_{CoM}\) \eqref{eq:detail_kinematics_cube_small}, the resulting design is shown in Figure \ref{fig:detail_kinematics_cubic_above_small}. + +\begin{equation}\label{eq:detail_kinematics_cube_small} + H_c < 2 H_{CoM} +\end{equation} + +This configuration is similar to that described in \cite{furutani04_nanom_cuttin_machin_using_stewar}, although they do not explicitly identify it as a cubic configuration. +Adjacent struts are parallel to each other, differing from the typical architecture where parallel struts are positioned opposite to each other. + +This approach yields a compact architecture, but the small cube size may result in insufficient rotational stiffness. + +\begin{figure}[htbp] +\begin{subfigure}{0.36\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_small_iso.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_small_iso}Isometric view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_small_side.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_small_side}Side view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_small_top.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_small_top}Top view} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_above_small}Cubic architecture with cube's center above the top platform. A cube height of 40mm is used.} +\end{figure} +\section{Medium sized cube} + +Increasing the cube's size such that \eqref{eq:detail_kinematics_cube_medium} is verified produces an architecture with intersecting struts (Figure \ref{fig:detail_kinematics_cubic_above_medium}). + +\begin{equation}\label{eq:detail_kinematics_cube_medium} + 2 H_{CoM} < H_c < 2 (H_{CoM} + H) +\end{equation} + +This configuration resembles the design proposed in \cite{yang19_dynam_model_decoup_contr_flexib} (Figure \ref{fig:detail_kinematics_yang19}), although their design is not strictly cubic. + +\begin{figure}[htbp] +\begin{subfigure}{0.36\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_medium_iso.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_medium_iso}Isometric view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_medium_side.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_medium_side}Side view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_medium_top.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_medium_top}Top view} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_above_medium}Cubic architecture with cube's center above the top platform. A cube height of 140mm is used.} +\end{figure} +\section{Large cube} + +When the cube's height exceeds twice the sum of the platform height and CoM height \eqref{eq:detail_kinematics_cube_large}, the architecture shown in Figure \ref{fig:detail_kinematics_cubic_above_large} is obtained. + +\begin{equation}\label{eq:detail_kinematics_cube_large} + 2 (H_{CoM} + H) < H_c +\end{equation} + +\begin{figure}[htbp] +\begin{subfigure}{0.36\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_large_iso.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_large_iso}Isometric view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_large_side.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_large_side}Side view} +\end{subfigure} +\begin{subfigure}{0.30\textwidth} +\begin{center} +\includegraphics[scale=1,width=0.9\linewidth]{figs/detail_kinematics_cubic_above_large_top.png} +\end{center} +\subcaption{\label{fig:detail_kinematics_cubic_above_large_top}Top view} +\end{subfigure} +\caption{\label{fig:detail_kinematics_cubic_above_large}Cubic architecture with cube's center above the top platform. A cube height of 240mm is used.} +\end{figure} +\section{Platform size} + +For the proposed configuration, the top joints \(\bm{b}_i\) (resp. the bottom joints \(\bm{a}_i\)) and are positioned on a circle with radius \(R_{b_i}\) (resp. \(R_{a_i}\)) described by Equation \eqref{eq:detail_kinematics_cube_joints}. + +\begin{subequations}\label{eq:detail_kinematics_cube_joints} +\begin{align} + R_{b_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 H_{CoM}^2} \label{eq:detail_kinematics_cube_top_joints} \\ + R_{a_i} &= \sqrt{\frac{3}{2} H_c^2 + 2 (H_{CoM} + H)^2} \label{eq:detail_kinematics_cube_bot_joints} +\end{align} +\end{subequations} + +Since the rotational stiffness for the cubic architecture scales with the square of the cube's height \eqref{eq:detail_kinematics_cubic_stiffness}, the cube's size can be determined based on rotational stiffness requirements. +Subsequently, using Equation \eqref{eq:detail_kinematics_cube_joints}, the dimensions of the top and bottom platforms can be calculated. +\chapter*{Conclusion} +The analysis of the cubic architecture for Stewart platforms yielded several important findings. +While the cubic configuration provides uniform stiffness in the XYZ directions, it stiffness property becomes particularly advantageous when forces and torques are applied at the cube's center. +Under these conditions, the stiffness matrix becomes diagonal, resulting in a decoupled Cartesian plant at low frequencies. + +Regarding mobility, the translational capabilities of the cubic configuration exhibit uniformity along the directions of the orthogonal struts, rather than complete uniformity in the Cartesian space. +This understanding refines the characterization of cubic architecture mobility commonly presented in literature. + +The analysis of decentralized control in the frame of the struts revealed more nuanced results than expected. +While cubic architectures are frequently associated with reduced coupling between actuators and sensors, this study showed that these benefits may be more subtle or context-dependent than commonly described. +Under the conditions analyzed, the coupling characteristics of cubic and non-cubic configurations, in the frame of the struts, appeared similar. + +Fully decoupled dynamics in the Cartesian frame can be achieved when the center of mass of the moving body coincides with the cube's center. +However, this arrangement presents practical challenges, as the cube's center is traditionally located between the top and bottom platforms, making payload placement problematic for many applications. + +To address this limitation, modified cubic architectures have been proposed with the cube's center positioned above the top platform. +Three distinct configurations have been identified, each with different geometric arrangements but sharing the common characteristic that the cube's center is positioned above the top platform. +This structural modification enables the alignment of the moving body's center of mass with the center of stiffness, resulting in beneficial decoupling properties in the Cartesian frame. +\printbibliography[heading=bibintoc,title={Bibliography}] +\end{document} diff --git a/paper/figs/detail_kinematics_centralized_control.pdf b/paper/figs/detail_kinematics_centralized_control.pdf new file mode 100644 index 0000000..2febd52 Binary files /dev/null and b/paper/figs/detail_kinematics_centralized_control.pdf differ diff --git a/paper/figs/detail_kinematics_centralized_control.png b/paper/figs/detail_kinematics_centralized_control.png new file mode 100644 index 0000000..cf6eae3 Binary files /dev/null and b/paper/figs/detail_kinematics_centralized_control.png differ diff --git a/paper/figs/detail_kinematics_centralized_control.svg b/paper/figs/detail_kinematics_centralized_control.svg new file mode 100644 index 0000000..08a523e --- /dev/null +++ b/paper/figs/detail_kinematics_centralized_control.svg @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/paper/figs/detail_kinematics_cubic_above_large_iso.pdf b/paper/figs/detail_kinematics_cubic_above_large_iso.pdf new file mode 100644 index 0000000..15bd222 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_iso.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_large_iso.png b/paper/figs/detail_kinematics_cubic_above_large_iso.png new file mode 100644 index 0000000..3a4359e Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_iso.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_large_side.pdf b/paper/figs/detail_kinematics_cubic_above_large_side.pdf new file mode 100644 index 0000000..3732070 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_side.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_large_side.png b/paper/figs/detail_kinematics_cubic_above_large_side.png new file mode 100644 index 0000000..ec5d7d1 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_side.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_large_top.pdf b/paper/figs/detail_kinematics_cubic_above_large_top.pdf new file mode 100644 index 0000000..15aa9fd Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_top.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_large_top.png b/paper/figs/detail_kinematics_cubic_above_large_top.png new file mode 100644 index 0000000..ebe15d6 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_large_top.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_iso.pdf b/paper/figs/detail_kinematics_cubic_above_medium_iso.pdf new file mode 100644 index 0000000..0d06402 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_iso.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_iso.png b/paper/figs/detail_kinematics_cubic_above_medium_iso.png new file mode 100644 index 0000000..1ef7810 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_iso.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_side.pdf b/paper/figs/detail_kinematics_cubic_above_medium_side.pdf new file mode 100644 index 0000000..c840f13 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_side.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_side.png b/paper/figs/detail_kinematics_cubic_above_medium_side.png new file mode 100644 index 0000000..f2bda0b Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_side.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_top.pdf b/paper/figs/detail_kinematics_cubic_above_medium_top.pdf new file mode 100644 index 0000000..4bf2062 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_top.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_medium_top.png b/paper/figs/detail_kinematics_cubic_above_medium_top.png new file mode 100644 index 0000000..2e9887c Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_medium_top.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_iso.pdf b/paper/figs/detail_kinematics_cubic_above_small_iso.pdf new file mode 100644 index 0000000..f599fb7 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_iso.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_iso.png b/paper/figs/detail_kinematics_cubic_above_small_iso.png new file mode 100644 index 0000000..e8db783 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_iso.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_side.pdf b/paper/figs/detail_kinematics_cubic_above_small_side.pdf new file mode 100644 index 0000000..c572d11 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_side.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_side.png b/paper/figs/detail_kinematics_cubic_above_small_side.png new file mode 100644 index 0000000..f82b719 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_side.png differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_top.pdf b/paper/figs/detail_kinematics_cubic_above_small_top.pdf new file mode 100644 index 0000000..e977ce6 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_top.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_above_small_top.png b/paper/figs/detail_kinematics_cubic_above_small_top.png new file mode 100644 index 0000000..0fcd528 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_above_small_top.png differ diff --git a/paper/figs/detail_kinematics_cubic_architecture_example.pdf b/paper/figs/detail_kinematics_cubic_architecture_example.pdf new file mode 100644 index 0000000..0c06e07 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_architecture_example.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_architecture_example.png b/paper/figs/detail_kinematics_cubic_architecture_example.png new file mode 100644 index 0000000..62c4a7e Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_architecture_example.png differ diff --git a/paper/figs/detail_kinematics_cubic_architecture_example_small.pdf b/paper/figs/detail_kinematics_cubic_architecture_example_small.pdf new file mode 100644 index 0000000..c791eda Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_architecture_example_small.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_architecture_example_small.png b/paper/figs/detail_kinematics_cubic_architecture_example_small.png new file mode 100644 index 0000000..6b936ac Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_architecture_example_small.png differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_cok.pdf b/paper/figs/detail_kinematics_cubic_cart_coupling_cok.pdf new file mode 100644 index 0000000..c292fc7 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_cok.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_cok.png b/paper/figs/detail_kinematics_cubic_cart_coupling_cok.png new file mode 100644 index 0000000..7afb5b6 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_cok.png differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_com.pdf b/paper/figs/detail_kinematics_cubic_cart_coupling_com.pdf new file mode 100644 index 0000000..c9777e4 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_com.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_com.png b/paper/figs/detail_kinematics_cubic_cart_coupling_com.png new file mode 100644 index 0000000..4a3f93c Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_com.png differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.pdf b/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.pdf new file mode 100644 index 0000000..9196e23 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.png b/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.png new file mode 100644 index 0000000..de990e1 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_cart_coupling_com_cok.png differ diff --git a/paper/figs/detail_kinematics_cubic_centered_payload.pdf b/paper/figs/detail_kinematics_cubic_centered_payload.pdf new file mode 100644 index 0000000..1848d37 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_centered_payload.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_centered_payload.png b/paper/figs/detail_kinematics_cubic_centered_payload.png new file mode 100644 index 0000000..0409158 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_centered_payload.png differ diff --git a/paper/figs/detail_kinematics_cubic_decentralized_dL.pdf b/paper/figs/detail_kinematics_cubic_decentralized_dL.pdf new file mode 100644 index 0000000..46e28de Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_decentralized_dL.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_decentralized_dL.png b/paper/figs/detail_kinematics_cubic_decentralized_dL.png new file mode 100644 index 0000000..b6c14e5 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_decentralized_dL.png differ diff --git a/paper/figs/detail_kinematics_cubic_decentralized_fn.pdf b/paper/figs/detail_kinematics_cubic_decentralized_fn.pdf new file mode 100644 index 0000000..18baafa Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_decentralized_fn.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_decentralized_fn.png b/paper/figs/detail_kinematics_cubic_decentralized_fn.png new file mode 100644 index 0000000..211fb9c Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_decentralized_fn.png differ diff --git a/paper/figs/detail_kinematics_cubic_mobility_rotations.pdf b/paper/figs/detail_kinematics_cubic_mobility_rotations.pdf new file mode 100644 index 0000000..b91f05c Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_mobility_rotations.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_mobility_rotations.png b/paper/figs/detail_kinematics_cubic_mobility_rotations.png new file mode 100644 index 0000000..e22d8c3 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_mobility_rotations.png differ diff --git a/paper/figs/detail_kinematics_cubic_mobility_translations.pdf b/paper/figs/detail_kinematics_cubic_mobility_translations.pdf new file mode 100644 index 0000000..8f8e2c4 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_mobility_translations.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_mobility_translations.png b/paper/figs/detail_kinematics_cubic_mobility_translations.png new file mode 100644 index 0000000..c0cfb5f Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_mobility_translations.png differ diff --git a/paper/figs/detail_kinematics_cubic_payload.pdf b/paper/figs/detail_kinematics_cubic_payload.pdf new file mode 100644 index 0000000..5bfe341 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_payload.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_payload.png b/paper/figs/detail_kinematics_cubic_payload.png new file mode 100644 index 0000000..975fa3c Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_payload.png differ diff --git a/paper/figs/detail_kinematics_cubic_schematic.pdf b/paper/figs/detail_kinematics_cubic_schematic.pdf new file mode 100644 index 0000000..5be3635 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_schematic.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_schematic.png b/paper/figs/detail_kinematics_cubic_schematic.png new file mode 100644 index 0000000..755cfbe Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_schematic.png differ diff --git a/paper/figs/detail_kinematics_cubic_schematic.svg b/paper/figs/detail_kinematics_cubic_schematic.svg new file mode 100644 index 0000000..7285128 --- /dev/null +++ b/paper/figs/detail_kinematics_cubic_schematic.svg @@ -0,0 +1,185 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/paper/figs/detail_kinematics_cubic_schematic_full.pdf b/paper/figs/detail_kinematics_cubic_schematic_full.pdf new file mode 100644 index 0000000..b789fc7 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_schematic_full.pdf differ diff --git a/paper/figs/detail_kinematics_cubic_schematic_full.png b/paper/figs/detail_kinematics_cubic_schematic_full.png new file mode 100644 index 0000000..f74ecc5 Binary files /dev/null and b/paper/figs/detail_kinematics_cubic_schematic_full.png differ diff --git a/paper/figs/detail_kinematics_cubic_schematic_full.svg b/paper/figs/detail_kinematics_cubic_schematic_full.svg new file mode 100644 index 0000000..cb8f0f7 --- /dev/null +++ b/paper/figs/detail_kinematics_cubic_schematic_full.svg @@ -0,0 +1,339 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/paper/figs/detail_kinematics_decentralized_control.pdf b/paper/figs/detail_kinematics_decentralized_control.pdf new file mode 100644 index 0000000..d65c206 Binary files /dev/null and b/paper/figs/detail_kinematics_decentralized_control.pdf differ diff --git a/paper/figs/detail_kinematics_decentralized_control.png b/paper/figs/detail_kinematics_decentralized_control.png new file mode 100644 index 0000000..999d34e Binary files /dev/null and b/paper/figs/detail_kinematics_decentralized_control.png differ diff --git a/paper/figs/detail_kinematics_decentralized_control.svg b/paper/figs/detail_kinematics_decentralized_control.svg new file mode 100644 index 0000000..0843787 --- /dev/null +++ b/paper/figs/detail_kinematics_decentralized_control.svg @@ -0,0 +1,109 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/paper/figs/detail_kinematics_non_cubic_decentralized_dL.pdf b/paper/figs/detail_kinematics_non_cubic_decentralized_dL.pdf new file mode 100644 index 0000000..bc50bbb Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_decentralized_dL.pdf differ diff --git a/paper/figs/detail_kinematics_non_cubic_decentralized_dL.png b/paper/figs/detail_kinematics_non_cubic_decentralized_dL.png new file mode 100644 index 0000000..bbf077a Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_decentralized_dL.png differ diff --git a/paper/figs/detail_kinematics_non_cubic_decentralized_fn.pdf b/paper/figs/detail_kinematics_non_cubic_decentralized_fn.pdf new file mode 100644 index 0000000..cb8dc7c Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_decentralized_fn.pdf differ diff --git a/paper/figs/detail_kinematics_non_cubic_decentralized_fn.png b/paper/figs/detail_kinematics_non_cubic_decentralized_fn.png new file mode 100644 index 0000000..b204328 Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_decentralized_fn.png differ diff --git a/paper/figs/detail_kinematics_non_cubic_payload.pdf b/paper/figs/detail_kinematics_non_cubic_payload.pdf new file mode 100644 index 0000000..28c6dbf Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_payload.pdf differ diff --git a/paper/figs/detail_kinematics_non_cubic_payload.png b/paper/figs/detail_kinematics_non_cubic_payload.png new file mode 100644 index 0000000..d6f6c63 Binary files /dev/null and b/paper/figs/detail_kinematics_non_cubic_payload.png differ diff --git a/paper/preamble.tex b/paper/preamble.tex new file mode 100644 index 0000000..7d9fb17 --- /dev/null +++ b/paper/preamble.tex @@ -0,0 +1,35 @@ +\usepackage[ % + acronym, % Separate acronyms and glossary + toc, % appear in ToC + automake, % auto-use the makeglossaries command (requires shell-escape) + nonumberlist, % don't back reference pages + nogroupskip, % don't group by letter + nopostdot % don't add a dot at the end of each element +]{glossaries} + +\usepackage[stylemods=longextra]{glossaries-extra} + +\setabbreviationstyle[acronym]{long-short} +\setglossarystyle{long-name-desc} + +\usepackage{amssymb} +\usepackage{amsmath} +\usepackage{cases} +\usepackage{empheq} + +\usepackage[binary-units=true]{siunitx} + +\sisetup{% + detect-all = true, + detect-family = true, + detect-mode = true, + detect-shape = true, + detect-weight = true, + detect-inline-weight = math, +} + +\DeclareSIUnit\px{px} +\DeclareSIUnit\rms{rms} + +\makeindex +\makeglossaries diff --git a/paper/preamble_extra.tex b/paper/preamble_extra.tex new file mode 100644 index 0000000..98cfc04 --- /dev/null +++ b/paper/preamble_extra.tex @@ -0,0 +1,134 @@ +\usepackage{float} +\usepackage{enumitem} + +\usepackage{caption,tabularx,booktabs} +\usepackage{bm} + +\usepackage{xpatch} % Recommanded for biblatex +\usepackage[ % use biblatex for bibliography + backend=biber, % use biber backend (bibtex replacement) or bibtex + style=ieee, % bib style + hyperref=true, % activate hyperref support + backref=true, % activate backrefs + isbn=false, % don't show isbn tags + url=false, % don't show url tags + doi=false, % don't show doi tags + urldate=long, % display type for dates + maxnames=3, % + minnames=1, % + maxbibnames=5, % + minbibnames=3, % + maxcitenames=2, % + mincitenames=1 % + ]{biblatex} + +\setlength\bibitemsep{1.1\itemsep} + +\usepackage{caption} +\usepackage{subcaption} + +\captionsetup[figure]{labelfont=bf} +\captionsetup[subfigure]{labelfont=bf} +\captionsetup[listing]{labelfont=bf} +\captionsetup[table]{labelfont=bf} + +\usepackage{xcolor} + +\definecolor{my-blue}{HTML}{6b7adb} +\definecolor{my-pale-blue}{HTML}{e6e9f9} +\definecolor{my-red}{HTML}{db6b6b} +\definecolor{my-pale-red}{HTML}{f9e6e6} +\definecolor{my-green}{HTML}{6bdbb6} +\definecolor{my-pale-green}{HTML}{e6f9f3} +\definecolor{my-yellow}{HTML}{dbd26b} +\definecolor{my-pale-yellow}{HTML}{f9f7e6} +\definecolor{my-orange}{HTML}{dba76b} +\definecolor{my-pale-orange}{HTML}{f9f0e6} +\definecolor{my-grey}{HTML}{a3a3a3} +\definecolor{my-pale-grey}{HTML}{f0f0f0} +\definecolor{my-turq}{HTML}{6bc7db} +\definecolor{my-pale-turq}{HTML}{e6f6f9} + +\usepackage{inconsolata} + +\usepackage[newfloat=true, chapter]{minted} +\usemintedstyle{autumn} + +\setminted{frame=lines,breaklines=true,tabsize=4,fontsize=\scriptsize,autogobble=true,labelposition=topline,bgcolor=my-pale-grey} +\setminted[matlab]{label=Matlab} +\setminted[latex]{label=LaTeX} +\setminted[bash]{label=Bash} +\setminted[python]{label=Python} +\setminted[text]{label=Results} +\setminted[md]{label=Org Mode} + +\setmintedinline{fontsize=\normalsize,bgcolor=my-pale-grey} + +\usepackage[most]{tcolorbox} + +\tcbuselibrary{minted} + +\newtcolorbox{seealso}{ enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=See Also} +\newtcolorbox{hint}{ enhanced,breakable,colback=my-pale-grey,colframe=my-grey,fonttitle=\bfseries,title=Hint} +\newtcolorbox{definition}{enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Definition} +\newtcolorbox{important}{ enhanced,breakable,colback=my-pale-red, colframe=my-red, fonttitle=\bfseries,title=Important} +\newtcolorbox{exampl}[1][]{ enhanced,breakable,colback=my-pale-green,colframe=my-green,fonttitle=\bfseries,title=Example,#1} +\newtcolorbox{exercice}{ enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Exercice} +\newtcolorbox{question}{ enhanced,breakable,colback=my-pale-yellow,colframe=my-yellow,fonttitle=\bfseries,title=Question} +\newtcolorbox{answer}{ enhanced,breakable,colback=my-pale-turq,colframe=my-turq,fonttitle=\bfseries,title=Answer} +\newtcolorbox{summary}{ enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Summary} +\newtcolorbox{note}{ enhanced,breakable,colback=my-pale-blue,colframe=my-blue,fonttitle=\bfseries,title=Note} +\newtcolorbox{caution}{ enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Caution} +\newtcolorbox{warning}{ enhanced,breakable,colback=my-pale-orange,colframe=my-orange,fonttitle=\bfseries,title=Warning} + +\newtcolorbox{my-quote}[1]{% + colback=my-pale-grey, + grow to right by=-10mm, + grow to left by=-10mm, + boxrule=0pt, + boxsep=0pt, + breakable, + enhanced jigsaw, + borderline west={4pt}{0pt}{my-grey}} + +\renewenvironment{quote}{\begin{my-quote}}{\end{my-quote}} + +\newtcolorbox{my-verse}[1]{% + colback=my-pale-grey, + grow to right by=-10mm, + grow to left by=-10mm, + boxrule=0pt, + boxsep=0pt, + breakable, + enhanced jigsaw, + borderline west={4pt}{0pt}{my-grey}} + +\renewenvironment{verse}{\begin{my-verse}}{\end{my-verse}} + +\usepackage{environ}% http://ctan.org/pkg/environ +\NewEnviron{aside}{% + \marginpar{\BODY} +} + +\renewenvironment{verbatim}{\VerbatimEnvironment\begin{minted}[]{text}}{\end{minted}} + +\usepackage{soul} +\sethlcolor{my-pale-grey} + +\let\OldTexttt\texttt +\renewcommand{\texttt}[1]{{\ttfamily\hl{\mbox{\,#1\,}}}} + +\makeatletter +\preto\Gin@extensions{png,} +\DeclareGraphicsRule{.png}{pdf}{.pdf}{\noexpand\Gin@base.pdf} +\preto\Gin@extensions{gif,} +\DeclareGraphicsRule{.gif}{png}{.png}{\noexpand\Gin@base.png} +\makeatother + +\usepackage{hyperref} +\hypersetup{ + colorlinks = true, + allcolors = my-blue +} + +\usepackage{hypcap}