2024-03-19 15:13:18 +01:00
#+TITLE : Simscape Model - Nano Hexapod
:DRAWER:
#+LANGUAGE : en
#+EMAIL : dehaeze.thomas@gmail.com
#+AUTHOR : Dehaeze Thomas
#+HTML_LINK_HOME : ../index.html
#+HTML_LINK_UP : ../index.html
#+HTML_HEAD : <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD : <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
#+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]
2025-02-05 17:47:37 +01:00
#+LATEX_HEADER : \input{preamble.tex}
#+LATEX_HEADER_EXTRA : \input{preamble_extra.tex}
2024-03-19 15:13:18 +01:00
#+LATEX_HEADER_EXTRA : \bibliography{simscape-nano-hexapod.bib}
#+BIND : org-latex-bib-compiler "biber"
#+PROPERTY : header-args:matlab :session *MATLAB*
#+PROPERTY : header-args:matlab+ :comments org
#+PROPERTY : header-args:matlab+ :exports none
#+PROPERTY : header-args:matlab+ :results none
#+PROPERTY : header-args:matlab+ :eval no-export
#+PROPERTY : header-args:matlab+ :noweb yes
#+PROPERTY : header-args:matlab+ :mkdirp yes
#+PROPERTY : header-args:matlab+ :output-dir figs
#+PROPERTY : header-args:matlab+ :tangle no
#+PROPERTY : header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY : header-args:latex+ :imagemagick t :fit yes
#+PROPERTY : header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY : header-args:latex+ :imoutoptions -quality 100
#+PROPERTY : header-args:latex+ :results file raw replace
#+PROPERTY : header-args:latex+ :buffer no
#+PROPERTY : header-args:latex+ :tangle no
#+PROPERTY : header-args:latex+ :eval no-export
#+PROPERTY : header-args:latex+ :exports results
#+PROPERTY : header-args:latex+ :mkdirp yes
#+PROPERTY : header-args:latex+ :output-dir figs
#+PROPERTY : header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:
#+latex : \clearpage
* Build :noexport:
#+NAME : startblock
#+BEGIN_SRC emacs-lisp :results none :tangle no
(add-to-list 'org-latex-classes
'("scrreprt"
"\\documentclass{scrreprt}"
("\\chapter{%s}" . "\\chapter*{%s}")
("\\section{%s}" . "\\section*{%s}")
("\\subsection{%s}" . "\\subsection*{%s}")
("\\paragraph{%s}" . "\\paragraph*{%s}")
))
;; Remove automatic org heading labels
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
"Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
(when (org-export-derived-backend-p backend 'latex)
(replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
(add-to-list 'org-export-filter-headline-functions
'my-latex-filter-removeOrgAutoLabels)
;; Remove all org comments in the output LaTeX file
(defun delete-org-comments (backend)
(loop for comment in (reverse (org-element-map (org-element-parse-buffer)
'comment 'identity))
do
(setf (buffer-substring (org-element-property :begin comment)
(org-element-property :end comment))
"")))
(add-hook 'org-export-before-processing-hook 'delete-org-comments)
;; Use no package by default
(setq org-latex-packages-alist nil)
(setq org-latex-default-packages-alist nil)
;; Do not include the subtitle inside the title
(setq org-latex-subtitle-separate t)
(setq org-latex-subtitle-format "\\subtitle{%s}")
(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
org-ref-acronyms-before-parsing))
#+END_SRC
* Notes :noexport:
2025-02-05 17:47:37 +01:00
** Notes
Prefix is =nhexa=
2024-03-19 15:13:18 +01:00
Based on:
2025-02-05 17:47:37 +01:00
- [ ] Stewart platform presentation: [[file:~/Cloud/meetings/group-meetings-me/2020-01-27-Stewart-Platform-Simscape/2020-01-27-Stewart-Platform-Simscape.org ]]
2024-03-19 15:13:18 +01:00
- [ ] Add some sections from here: [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/index.org ]]
For instance:
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/stewart-architecture.org ][stewart architecture ]]
2025-02-05 17:47:37 +01:00
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/stewart_platform.org::+TITLE: Stewart Platform - Simscape Model ]]
2024-03-19 15:13:18 +01:00
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org ][kinematic study ]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/identification.org ][stewart platform - dynamics ]]
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org ][cubic configuration ]]
- [ ] Look at the [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org ][NASS 2020 report ]]
2025-02-05 17:47:37 +01:00
Sections 5.1, 5.4
2024-03-19 15:13:18 +01:00
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/amplified_piezoelectric_stack.org ][amplified_piezoelectric_stack ]] (Just use 2DoF here)
2025-02-05 17:47:37 +01:00
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/nano_hexapod.org ][nano_hexapod ]] (it seems this report is already after the detailed design phase: yes but some parts could be interesting)
2024-03-19 15:13:18 +01:00
- [ ] Should the study of effect of flexible joints be included here?
2025-02-05 17:47:37 +01:00
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org
Questions:
- [ ] The APA model should maybe not be used here, same for the nice top and bottom plates. Here the detailed design is not yet performed
2025-02-06 16:02:34 +01:00
** DONE [#A] Copy relevant parts of reports
CLOSED: [2025-02-06 Thu 15:27]
2025-02-05 17:47:37 +01:00
2025-02-06 16:02:34 +01:00
- [X] Stewart platform presentation: [[file:~/Cloud/meetings/group-meetings-me/2020-01-27-Stewart-Platform-Simscape/2020-01-27-Stewart-Platform-Simscape.org ]]
- [X] Add some sections from here: [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/index.org ]]
2025-02-05 17:47:37 +01:00
For instance:
2025-02-06 16:02:34 +01:00
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/stewart-architecture.org ][stewart architecture ]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/stewart_platform.org::+TITLE: Stewart Platform - Simscape Model ]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org ][kinematic study ]]
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/identification.org ]]
Effect of joints stiffnesses
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org ][cubic configuration ]]
Not relevant here: in chapter 2
- [X] Look at the [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org ][NASS 2020 report ]]
2025-02-05 17:47:37 +01:00
Sections 5.1, 5.4
2025-02-06 16:02:34 +01:00
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/amplified_piezoelectric_stack.org ][amplified_piezoelectric_stack ]] (Just use 2DoF here)
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/nano_hexapod.org ][nano_hexapod ]] (it seems this report is already after the detailed design phase: yes but some parts could be interesting)
*Will also be used in Chapter 2*
- [X] Should the study of effect of flexible joints be included here?
*No, considered perfect and then optimized in chapter 2*
2025-02-05 17:47:37 +01:00
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org
** DONE [#A] Make a nice outline
CLOSED: [2025-02-05 Wed 17:45]
*Introduction*
- Choice of architecture to do 5DoF control
- Stewart platform
- Need to model the active vibration platform
- Control
*1 - Active Vibration Platforms* :
Introduction:
Maybe no sections, just a review discussing several aspect of the platforms.
1. Review of active vibration platforms (focused on Synchrotron applications)
2. Serial and Parallel Architecture: advantages and disadvantages of both
3. Which architecture => Parallel manipulator? Why *Stewart platform* ?
*2 - The Stewart Platform* :
Introduction: some history about Stewart platform and why it is so used
1. Architecture (plates, struts, joints)
2. Kinematics and Jacobian
4. Static Analysis
5. Dynamic Analysis: very complex => multi-body model
For instance, compute the plant for massless struts and perfect joints (will be compared with Simscape model).
But say that if we want to model more complex cases, it becomes impractical (cite papers).
*3 - Multi-Body model of the Stewart platform* :
Introduction: Complex dynamics => analytical formulas can be complex = > Choose to study the dynamics using a multi-body model
1. Model definition: (Matlab Toolbox), frames, inertias of parts, stiffnesses, struts, etc...
2. Joints: perfect 2dof/3dof (+ mass-less)
3. Actuators: APA + Encoder (mass-less)
4. Nano-Hexapod: definition of each part + Plant with defined inputs/outputs (force sensor, relative displacement sensor, etc...)
Compare with analytical formulas (see number of states)
*4 - Control of the Stewart Platform* :
Introduction: MIMO control => much more complex than SISO control because of interaction. Possible to ignore interaction when good decoupling (important to have tools to study interaction)
1. Centralized and Decentralized Control
2. Decoupling Control / Choice of control space file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org
Estimate coupling: RGA
- Jacobian matrices, CoK, CoM, control in the frame of the struts, ...
- Discussion of cubic architecture (quick, as it is going to be in detailed in chapter 2)
- SVD, Modal, ...
3. Active Damping: decentralized IFF
Guaranteed stability?
For decentralized control: "MIMO root locus"
How to optimize the added damping to all modes?
4. HAC-LAC
Stability of closed-loop: Nyquist (main advantage: possible to do with experimental FRF)
*Conclusion* :
- Configurable Stewart platform model
- Will be included in the multi-body model of the micro-station => nass multi body model
** DONE [#A] Location of this report in the complete thesis
CLOSED: [2025-02-05 Wed 16:04]
*Before the report* (assumptions):
- Uniaxial model: no stiff actuator, HAC-LAC strategy
- Rotating model:
Soft actuators are problematic due to gyroscopic effects
Use moderately stiff (1um/N).
IFF can be applied with APA architecture
- Model of Micro-station is ready
*In this report* :
- Goal: build a flexible (i.e. configurable) multi-body model of a Stewart platform that will be used in the next section to perform dynamical analysis and simulate experiments with the complete NASS
- Here, I propose to work with "perfect" stewart platforms:
- almost mass-less struts
- joints with zero stiffness in free DoFs (i.e. 2-DoF and 3-DoF joints)
- Presentation of Stewart platforms (Literature review about stewart platforms will be done in chapter 2)
- Presentation of the Simscape model
*After the report* (NASS-Simscape):
- nano-hexapod on top of micro-station
- control is performed
- simulations => validation of the concept
2025-02-06 16:02:34 +01:00
** TODO [#A] Make sure the Simulink file for the Stewart platform is working well
SCHEDULED: <2025-02-06 Thu>
It should be the exact model reference that will be included in the NASS model.
** DONE [#C] First time in the report that we speak about MIMO control ? Or maybe next section!
CLOSED: [2025-02-06 Thu 16:01]
2025-02-05 17:47:37 +01:00
Maybe should introduce:
- "MIMO" Root locus
- "MIMO" Nyquist plot / characteristic loci
Or should this be in annexes?
Maybe say that in this phd-thesis, the focus is not on the control.
I tried multiple architectures (complementary filters, etc.), but the focus is not on that.
2025-02-06 16:02:34 +01:00
** ANSW [#C] Cubic architecture should be the topic here or in the detailed design?
CLOSED: [2025-02-06 Thu 16:01]
2025-02-05 17:47:37 +01:00
I suppose that it should be in the detailed design phase.
(Review about Stewart platform design should be made in Chapter two.)
Here, just use simple control architecture for general validation (and not optimization).
2025-02-06 16:02:34 +01:00
** ANSW [#C] Should I make a review of control strategies?
CLOSED: [2025-02-06 Thu 16:01]
2025-02-05 17:47:37 +01:00
Yes it seems to good location for review related to control.
Jacobian matrix.
Control is the frame of the struts, in the cartesian frame (CoM, CoK), modal control, etc...
[[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org ][file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org ]]
2025-02-06 16:02:34 +01:00
** DONE [#C] Compare simscape =linearize= and analytical formula
CLOSED: [2025-02-06 Thu 16:01]
2025-02-05 17:47:37 +01:00
- [X] OK for $\omega=0$ (using just the Stiffness matrix)
- [ ] Should add the mass matrix and compare for all frequencies
The analytical dynamic model is taken from cite:taghirad13_paral
2025-02-06 16:02:34 +01:00
** DONE [#C] Output the cubic configuration with clear display of the cube and center of the cube
CLOSED: [2025-02-06 Thu 16:02]
2025-02-05 17:47:37 +01:00
[[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org ][cubic configuration ]]
2025-02-06 16:02:34 +01:00
*No, this will be in Chapter 2*
2025-02-05 17:47:37 +01:00
2025-02-06 16:02:34 +01:00
** CANC [#C] Maybe make an appendix to present the developed toolbox?
CLOSED: [2025-02-06 Thu 16:02]
2024-03-19 15:13:18 +01:00
2025-02-06 16:02:34 +01:00
- State "CANC" from "TODO" [2025-02-06 Thu 16:02]
2024-03-19 15:13:18 +01:00
* Introduction :ignore:
2025-02-06 16:02:34 +01:00
Now that the multi-body model of the micro-station has been developed and validated using dynamical measurements, a model of the active vibration platform can be integrated.
First, the mechanical architecture of the active platform needs to be carefully chosen.
In Section ref:sec:nhexa_platform_review, a quick review of active vibration platforms is performed.
The chosen architecture is the Stewart platform, which is presented in Section ref:sec:nhexa_stewart_platform.
It is a parallel manipulator that require the use of specific tools to study its kinematics.
However, to study the dynamics of the Stewart platform, the use of analytical equations is very complex.
Instead, a multi-body model of the Stewart platform is developed (Section ref:sec:nhexa_model), that can then be easily integrated on top of the micro-station's model.
From a control point of view, the Stewart platform is a MIMO system with complex dynamics.
To control such system, it requires several tools to study interaction (Section ref:sec:nhexa_control).
2025-02-05 17:47:37 +01:00
* Active Vibration Platforms
<<sec:nhexa_platform_review >>
** Introduction :ignore:
*Goals* :
2025-02-06 16:02:34 +01:00
- Quick review of active vibration platforms (5 or 6DoF) similar to NASS
2025-02-05 17:47:37 +01:00
- Explain why Stewart platform architecture is chosen
2025-02-06 16:02:34 +01:00
- Wanted controlled DOF: Y, Z, Ry
- But because of continuous rotation (key specificity): X,Y,Z,Rx,Ry in the frame of the active platform
2025-02-05 17:47:37 +01:00
- Literature review? (*maybe more suited for chapter 2* )
- file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org
- Talk about flexible joint? Maybe not so much as it should be topic of second chapter.
Just say that we must of flexible joints that can be defined as 3 to 6DoF joints, and it will be optimize in chapter 2.
- [[cite:&taghirad13_paral ]]
2025-02-06 16:02:34 +01:00
2025-02-05 17:47:37 +01:00
- For some systems, just XYZ control (stack stages), example: holler
- For other systems, Stewart platform (ID16a), piezo based
- Examples of Stewart platforms for general vibration control, some with Piezo, other with Voice coil. IFF, ...
Show different geometry configuration
- DCM: tripod?
** Active vibration control of sample stages
[[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A0-nass-introduction/nass-introduction.org::*Review of stages with online metrology for Synchrotrons ][Review of stages with online metrology for Synchrotrons ]]
- [ ] Talk about external metrology?
2025-02-06 16:02:34 +01:00
Maybe not the topic here.
2025-02-05 17:47:37 +01:00
- [ ] Talk about control architecture?
- [ ] Comparison with the micro-station / NASS
** Serial and Parallel Manipulators
*Goal* :
- Explain why a parallel manipulator is here preferred
- Compact, 6DoF, higher control bandwidth, linear, simpler
- Show some example of serial and parallel manipulators
- A review of Stewart platform will be given in Chapter related to the detailed design of the Nano-Hexapod
2025-02-06 16:02:34 +01:00
#+name : tab:nhexa_serial_vs_parallel
#+caption : Advantages and Disadvantages of both serial and parallel robots
#+attr_latex : :environment tabularx :width \linewidth :align lXX
#+attr_latex : :center t :booktabs t :float t
| | *Serial Robots* | *Parallel Robots* |
|--------------------+-----------------+-------------------|
| Advantages | Large Workspace | High Stiffness |
| Disadvantages | Low Stiffness | Small Workspace |
| Kinematic Struture | Open | Closed-loop |
2025-02-05 17:47:37 +01:00
* The Stewart platform
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_1_stewart_platform.m
:END:
<<sec:nhexa_stewart_platform >>
** Introduction :ignore:
# Most of this section is based on [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org][kinematic-study.org]]
- Some history about Stewart platforms
- What is so special and why it is so used in different fields: give examples
Explain advantages compared to serial architecture
- Little review (very q uick: two extreme sizes, piezo + voice coil)
Complete review of Stewart platforms will be made in Chapter 2
- Presentation of tools used to analyze the properties of the Stewart platform => useful for design and control
2025-02-06 16:02:34 +01:00
The Stewart Platform is very adapted for the NASS application for the following reasons:
- it is a fully parallel manipulator, thus all the motions errors can be compensated
- it is very compact compared to a serial manipulator
- it has high stiffness and good dynamic performances
The main disadvantage of Stewart platforms is the small workspace when compare the serial manipulators which is not a problem here.
2025-02-05 17:47:37 +01:00
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path >>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle >>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other >>
#+end_src
** Mechanical Architecture
<<ssec:nhexa_stewart_platform_architecture >>
2025-02-06 16:02:34 +01:00
- [ ] Use this file as a reference: file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/stewart-architecture.org
2024-03-19 15:13:18 +01:00
2025-02-05 17:47:37 +01:00
Presentation of the typical architecture
- Explain the different frames, etc...
- explain key elements:
- two plates
- joints
- actuators
2024-03-19 15:13:18 +01:00
2025-02-06 16:02:34 +01:00
[[file:figs/nhexa_stewart_platform_conf.png ]]
The Stewart Platform:
- Has 6 degrees-of-freedom
- Is a *Fully* parallel manipulator as the number of actuators is equal to the number of dof
- Is a *Symmetrical* parallel manipulator as all the struts are the same
#+name : tab:stewart_platforms_configurations
#+attr_latex : :environment tabularx :width \linewidth :align cXXX
#+attr_latex : :center t :booktabs t :float t
| | *Base Joint* | *Actuator Joint* | *Top Joint* |
|---------+--------------+------------------+-------------|
| *6-SPS* | Spherical | Prismatic | Spherical |
| *6-UPS* | Universal | Prismatic | Spherical |
2025-02-05 17:47:37 +01:00
Make well defined notations.
- {F}, {M}
- si, li, ai, bi, etc.
- [ ] Make figure with defined frames, joints, etc...
2025-02-06 16:02:34 +01:00
These frames are used to describe the relative motion of the two platforms through the position vector ${}^A\bm{P}_B$ of $\{B\}$ expressed in $\{A\}$ and the rotation matrix ${}^A\bm{R}_B$ expressing the orientation of $\{B\}$ with respect to $\{A\}$.
For the nano-hexapod, these frames are chosen to be located at the theoretical focus point of the X-ray light (xxx mm above the top platform).
#+begin_quote
Stewart platforms are generated in multiple steps.
We define 4 important *frames* :
- $\{F\}$: Frame fixed to the *Fixed* base and located at the center of its bottom surface.
This is used to fix the Stewart platform to some support.
- $\{M\}$: Frame fixed to the *Moving* platform and located at the center of its top surface.
This is used to place things on top of the Stewart platform.
- $\{A\}$: Frame fixed to the fixed base.
It defined the center of rotation of the moving platform.
- $\{B\}$: Frame fixed to the moving platform.
The motion of the moving platforms and forces applied to it are defined with respect to this frame $\{B\}$.
Then, we define the *location of the spherical joints* :
- $\bm{a}_{i}$ are the position of the spherical joints fixed to the fixed base
- $\bm{b}_{i}$ are the position of the spherical joints fixed to the moving platform
We define the *rest position* of the Stewart platform:
- For simplicity, we suppose that the fixed base and the moving platform are parallel and aligned with the vertical axis at their rest position.
- Thus, to define the rest position of the Stewart platform, we just have to defined its total height $H$.
$H$ corresponds to the distance from the bottom of the fixed base to the top of the moving platform.
From $\bm{a}_{i}$ and $\bm{b}_ {i}$, we can determine the *length and orientation of each strut* :
- $l_{i}$ is the length of the strut
- ${}^{A}\hat{\bm{s}}_{i}$ is the unit vector align with the strut
#+end_quote
2025-02-05 17:47:37 +01:00
** Kinematic Analysis
<<ssec:nhexa_stewart_platform_kinematics >>
2025-02-06 16:02:34 +01:00
**** Introduction :ignore:
Kinematic analysis refers to the study of the geometry of motion of a robot, without considering the forces that cause the motion.
The relation between the geometry of the manipulator with the final motion of the moving platform is derived and analyzed.
*Definition of the geometry of the Stewart Platform* :
- $\bm{a}_i$: position of the attachment points on the fixed base
- $\bm{b}_i$: position of moving attachment points
- $l_i$: length of each limb
- $\hat{\bm{s}}_i$: unit vector representing the direction of each limb
#+name : fig:nhexa_stewart_schematic
#+caption : Geometry of a Stewart Platform
#+attr_latex : :scale 1
[[file:figs/nhexa_stewart_schematic.png ]]
**** Loop Closure
At the displacement level, the *closure of each kinematic loop* can be express in the vector form as
\[ \vec{AB} = \vec{AA_i} + \vec{A_iB_i} - \vec{BB_i} \quad \text{for } i = 1,2,\dots,n \]
in which $\vec{AA_i}$ and $\vec{BB_i}$ can be easily obtained from the geometry of the attachment points in the base and in the moving platform.
The *loop closure* can be written as the unknown pose variables ${}^A\bm{P}$ and ${}^A\bm{R}_B$, the position vectors describing the known geometry of the base and of the moving platform, $\bm{a}_i$ and $\bm{b}_i$, and the limb vector $l_i {}^A\hat{\bm{s}}_i$:
\begin{equation*}
{}^A\bm{P} = {}^A\bm{a}_i + l_i{}^A\hat{\bm{s}}_i - {}^A\bm{R}_B {}^B\bm{b}_i \quad \text{for } i=1,2,\dots,n
\end{equation*}
2025-02-05 17:49:45 +01:00
**** Inverse Kinematics
2025-02-05 17:47:37 +01:00
2025-02-06 16:02:34 +01:00
For /inverse kinematic analysis/ , it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables $\bm{\mathcal{L}} = \left[ l_1, l_2, l_3, l_4, l_5, l_6 \right]^T$.
This problem can be easily solved using the loop closures.
The obtain joint variables are:
\begin{equation*}
\begin{aligned}
l_i = &\Big[ {}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + \dots\\
&2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i \Big]^{1/2}
\end{aligned}
\end{equation*}
If the position and orientation of the platform lie in the feasible workspace, the solution is unique.
Otherwise, the solution gives complex numbers.
2025-02-05 17:49:45 +01:00
**** Forward Kinematics
2025-02-05 17:47:37 +01:00
2025-02-06 16:02:34 +01:00
In /forward kinematic analysis/ , it is assumed that the vector of limb lengths $\bm{L}$ is given and the problem is to find the position ${}^A\bm{P}$ and the orientation ${}^A\bm{R}_B$.
This is a difficult problem that requires to solve nonlinear equations.
In a next section, an approximate solution of the forward kinematics problem is proposed for small displacements.
** The Jacobian Matrix
**** Introduction :ignore:
In vector calculus, the Jacobian matrix of a vector-valued function in several variables is the matrix of all its first-order partial derivatives.
Suppose $\bm{f}: \mathbb{R}^n \rightarrow \mathbb{R}^m$ is a function such that each of its first-order partial derivatives exist on $\mathbb{R}^n$.
This function takes a point $\bm{x} \in \mathbb{R}^n$ as input and produces the vector $\bm{f}(\bm{x}) \in \mathbb{R}^m$ as output.
Then the Jacobian matrix $\bm{J}$ of $\bm{f}$ is defined to be an $n \times m$ matrix, whose its (i,j)'th entry is $J_{ij} = \frac{\partial f_i}{\partial x_j}$.
The Jacobian matrix is the *linear transformation* that best approximates $\bm{f}$ for points close to $\bm{x}$.
*Summary* : Linear approximation of a function with several inputs and outputs around a working point.
**** Jacobian Computation - Velocity Loop Closure
Let's note:
- $\bm{\mathcal{L}} = \left[ l_1, l_2, \ldots, l_6 \right]^T$: vector of actuated joint coordinates
- $\bm{\mathcal{X}} = \left[ {}^A\bm{P}, \bm{}^A\hat{\bm{s}} \right]^T$: vector of platform motion variables
$\bm{\mathcal{L}}$ and $\bm{\mathcal{X}}$ are related through a system of /nonlinear algebraic equations/ representing the /kinematic constraints imposed by the struts/ , which can be generally written as $f(\bm{\mathcal{L}}, \bm{\mathcal{X}}) = 0$.
We can differentiate this equation with respect to time and obtain:
\begin{equation*}
\bm{J}_x \dot{\bm{\mathcal{X}}} = \bm{J}_l \dot{\bm{\mathcal{L}}} \quad \text{where} \quad
\bm{J}_x = \frac{\partial f}{\partial \bm{\mathcal{X}}} \quad \text{and} \quad \bm{J}_l = -\frac{\partial f}{\partial \bm{\mathcal{L}}}
\end{equation*}
With:
- $\dot{\bm{\mathcal{L}}} = [ \dot{l}_1, \dot{l}_2, \dot{l}_3, \dot{l}_4, \dot{l}_5, \dot{l}_6 ]^T$
- $\dot{\bm{X}} = [^A\bm{v}_p, {}^A\bm{\omega}]^T$:
2025-02-05 17:47:37 +01:00
2025-02-06 16:02:34 +01:00
The *general Jacobian matrix* is defined as:
\begin{equation*}
\dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}} \quad \text{with} \quad \bm{J} = {\bm{J}_l}^{-1} \bm{J}_x
\end{equation*}
The *velocity loop closures* are used for *obtaining the Jacobian matrices* in a straightforward manner:
\begin{align*}
{}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i = l_i {}^A\hat{\bm{s}}_i + {}^A\bm{a}_i
& \underset{\frac{\partial}{\partial t}}{\rightarrow}
{}^A\bm{v}_p + {}^A \dot{\bm{R}}_B {}^B\bm{b}_i = \dot{l}_i {}^A\hat{\bm{s}}_i + l_i {}^A\dot{\hat{\bm{s}}}_i \\
& \Leftrightarrow\hat{\bm{s}}_i {}^A\bm{v}_p + ({}^A\bm{b}_i \times \hat{\bm{s}}_i) {}^A\bm{\omega} = \dot{l}_i
\end{align*}
We can rearrange the equations in a matrix form:
\[ \dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}} \quad \text{with} \ \dot{\bm{\mathcal{L}}} = [ \dot{l}_1 \ \dots \ \dot{l}_6 ]^T \ \text{and} \ \dot{\bm{\mathcal{X}}} = [{}^A\bm{v}_p ,\ {}^A\bm{\omega}]^T \]
\begin{equation}
\bm{J} = \begin{bmatrix}
{\hat{\bm{s}}_1}^T & (\bm{b}_1 \times \hat{\bm{s}}_1)^T \\
{\hat{\bm{s}}_2}^T & (\bm{b}_2 \times \hat{\bm{s}}_2)^T \\
{\hat{\bm{s}}_3}^T & (\bm{b}_3 \times \hat{\bm{s}}_3)^T \\
{\hat{\bm{s}}_4}^T & (\bm{b}_4 \times \hat{\bm{s}}_4)^T \\
{\hat{\bm{s}}_5}^T & (\bm{b}_5 \times \hat{\bm{s}}_5)^T \\
{\hat{\bm{s}}_6}^T & (\bm{b}_6 \times \hat{\bm{s}}_6)^T
\end{bmatrix}
\end{equation}
$\bm{J}$ then depends only on:
- $\hat{\bm{s}}_i$ the orientation of the limbs expressed in $\{A\}$
- $\bm{b}_i$ the position of the joints with respect to $O_B$ and express in $\{A\}$
**** Approximate solution of the Forward and Inverse Kinematic problems
For small displacements mobile platform displacement $\delta \bm{\mathcal{X}} = [\delta x, \delta y, \delta z, \delta \theta_x, \delta \theta_y, \delta \theta_z ]^T$ around $\bm{\mathcal{X}}_0$, the associated joint displacement can be computed using the Jacobian (approximate solution of the inverse kinematic problem):
\begin{equation*}
\delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}}
\end{equation*}
Similarly, for small joint displacements $\delta\bm{\mathcal{L}} = [ \delta l_1,\ \dots,\ \delta l_6 ]^T$ around $\bm{\mathcal{L}}_0$, it is possible to find the induced small displacement of the mobile platform (approximate solution of the forward kinematic problem):
\begin{equation*}
\delta\bm{\mathcal{X}} = \bm{J}^{-1} \delta\bm{\mathcal{L}}
\end{equation*}
These two relations solve the forward and inverse kinematic problems for small displacement in a /approximate/ way.
As the inverse kinematic can be easily solved exactly this is not much useful, however, as the forward kinematic problem is difficult to solve, this approximation can be very useful for small displacements.
**** Range validity of the approximate inverse kinematics
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org::*Estimation of the range validity of the approximate inverse kinematics ][Estimation of the range validity of the approximate inverse kinematics ]]
**** Static Forces
The *principle of virtual work* states that the total virtual work, $\delta W$, done by all actuators and external forces is equal to zero:
\begin{align*}
\delta W &= \bm{\tau}^T \delta \bm{\mathcal{L}} - \bm{\mathcal{F}}^T \delta \bm{\mathcal{X}}\\
& = 0
\end{align*}
If we note:
- $\bm{\tau} = [\tau_1, \tau_2, \cdots, \tau_6]^T$: vector of actuator forces applied in each strut
- $\bm{\mathcal{F}} = [\bm{f}, \bm{n}]^T$: external force/torque action on the mobile platform at $\bm{O}_B$
From the definition of the Jacobian ($\delta \bm{\mathcal{L}} = \bm{J} \cdot \delta \bm{\mathcal{X}}$), we have $\left( \bm{\tau}^T \bm{J} - \bm{\mathcal{F}}^T \right) \delta \bm{\mathcal{X}} = 0$ that holds for any $\delta \bm{\mathcal{X}}$, hence:
\[ \bm{\tau}^T \bm{J} - \bm{\mathcal{F}}^T = 0 \quad \Rightarrow \quad \tcmbox{\bm{\mathcal{F}} = \bm{J}^T \bm{\tau}} \quad \text{and} \quad \tcmbox{\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}} \]
2025-02-05 17:47:37 +01:00
2025-02-05 17:49:45 +01:00
**** Singularities
2025-02-05 17:47:37 +01:00
- Briefly mention singularities, and say that for small stroke, it is not an issue, the Jacobian matrix may be considered constant
** Static Analysis
<<ssec:nhexa_stewart_platform_static >>
How stiffness varies with orientation of struts.
Same with stroke?
Or maybe in the detailed chapter?
2025-02-06 16:02:34 +01:00
The stiffness of the actuator $k_i$ links the applied actuator force $\delta \tau_i$ and the corresponding small deflection $\delta l_i$:
\begin{equation*}
\tau_i = k_i \delta l_i, \quad i = 1,\ \dots,\ 6
\end{equation*}
If we combine these 6 relations:
\begin{equation*}
\bm{\tau} = \mathcal{K} \delta \bm{\mathcal{L}} \quad \mathcal{K} = \text{diag}\left[ k_1,\ \dots,\ k_6 \right]
\end{equation*}
Substituting $\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}$ and $\delta \bm{\mathcal{L}} = \bm{J} \cdot \delta \bm{\mathcal{X}}$ gives
\begin{equation*}
\bm{\mathcal{F}} = \bm{J}^T \mathcal{K} \bm{J} \cdot \delta \bm{\mathcal{X}}
\end{equation*}
And then we identify the stiffness matrix $\bm{K}$:
\begin{equation*}
\bm{K} = \bm{J}^T \mathcal{K} \bm{J}
\end{equation*}
If the stiffness matrix $\bm{K}$ is inversible, the *compliance matrix* of the manipulator is defined as
\begin{equation*}
\bm{C} = \bm{K}^{-1} = (\bm{J}^T \mathcal{K} \bm{J})^{-1}
\end{equation*}
The compliance matrix of a manipulator shows the mapping of the moving platform wrench applied at $\bm{O}_B$ to its small deflection by
\begin{equation*}
\delta \bm{\mathcal{X}} = \bm{C} \cdot \bm{\mathcal{F}}
\end{equation*}
2025-02-05 17:47:37 +01:00
** Dynamic Analysis
<<ssec:nhexa_stewart_platform_dynamics >>
Very complex => multi-body model
For instance, compute the plant for massless struts and perfect joints (will be compared with Simscape model).
But say that if we want to model more complex cases, it becomes impractical (cite papers).
** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
2025-02-06 16:02:34 +01:00
Dynamic analysis of parallel manipulators presents an *inherent complexity due to their closed-loop structure and kinematic constraints* .
2025-02-05 17:47:37 +01:00
All depends on the geometry.
Reasonable choice of geometry is made in chapter 1.
Optimization of the geometry will be made in chapter 2.
* Multi-Body Model
2024-03-19 15:13:18 +01:00
:PROPERTIES:
2025-02-05 17:47:37 +01:00
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_2_model.m
2024-03-19 15:13:18 +01:00
:END:
2025-02-05 17:47:37 +01:00
<<sec:nhexa_model >>
2024-03-19 15:13:18 +01:00
** Introduction :ignore:
2025-02-05 17:47:37 +01:00
*Goal* :
- Study the dynamics of Stewart platform
- Instead of working with complex analytical models: a multi-body model is used.
Complex because has to model the inertia of the struts.
Cite papers that tries to model the stewart platform analytically
Advantage: it will be easily included in the model of the NASS
- Mention the Toolbox (maybe make a DOI for that)
- [ ] Have a table somewhere that summarizes the main characteristics of the nano-hexapod model
- location of joints
- size / mass of platforms, etc...
2024-03-19 15:13:18 +01:00
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path >>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle >>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other >>
#+end_src
2025-02-05 17:47:37 +01:00
** Model Definition
<<ssec:nhexa_model_def >>
- [ ] Make a schematic of the definition process (for instance knowing the ai, bi points + {A} and {B} allows to compute Jacobian, etc...)
- What is important for the model:
- Inertia of plates and struts
- Positions of joints / Orientation of struts
- Definition of frames (for Jacobian, stiffness analysis, etc...)
Then, several things can be computed:
- Kinematics, stiffness, platform mobility, dynamics, etc...
- Joints: can be 2dof to 6dof
- Actuators: can be modelled as wanted
** Nano Hexapod
<<ssec:nhexa_model_nano_hexapod >>
Start simple:
- Perfect joints, massless actuators
Joints: perfect 2dof/3dof (+ mass-less)
Actuators: APA + Encoder (mass-less)
- k = 1N/um
- Force sensor
Definition of each part + Plant with defined inputs/outputs (force sensor, relative displacement sensor, etc...)
** Model Dynamics
<<ssec:nhexa_model_dynamics >>
- If all is perfect (mass-less struts, perfect joints, etc...), maybe compare analytical model with simscape model?
- Say something about the model order
Model order is 12, and that we can compute modes from matrices M and K, compare with the Simscape model
- Compare with analytical formulas (see number of states)
** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
- Validation of multi-body model in a simple case
- Possible to increase the model complexity when required
- If considered 6dof joint stiffness, model order increases
- Can have an effect on IFF performances: [[cite:&preumont07_six_axis_singl_stage_activ ]]
- Conclusion: during the conceptual design, we consider a perfect, but will be taken into account later
- Optimization of the Flexible joint will be performed in Chapter 2.2
- MIMO system: how to control? => next section
* Control of Stewart Platforms
:PROPERTIES:
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_3_control.m
:END:
<<sec:nhexa_control >>
** Introduction :ignore:
MIMO control: much more complex than SISO control because of interaction.
Possible to ignore interaction when good decoupling is achieved.
Important to have tools to study interaction
Different ways to try to decouple a MIMO plant.
Reference book: [[cite:&skogestad07_multiv_feedb_contr ]]
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir >>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init >>
#+end_src
#+begin_src matlab :tangle no :noweb yes
<<m-init-path >>
#+end_src
#+begin_src matlab :eval no :noweb yes
<<m-init-path-tangle >>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other >>
#+end_src
** Centralized and Decentralized Control
<<ssec:nhexa_control_centralized_decentralized >>
- Explain what is centralized and decentralized:
- linked to the sensor position relative to the actuators
- linked to the fact that sensors and actuators pairs are "independent" or each other (related to the control architecture, not because there is no coupling)
- When can decentralized control be used and when centralized control is necessary?
Study of interaction: RGA
** Choice of the control space
<<ssec:nhexa_control_space >>
- [ ] file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org
- Jacobian matrices, CoK, CoM, control in the frame of the struts, SVD, Modal, ...
- Combined CoM and CoK => Discussion of cubic architecture ? (quick, as it is going to be in detailed in chapter 2)
- Explain also the link with the setpoint: it is interesting to have the controller in the frame of the performance variables
Also speak about disturbances? (and how disturbances can be mixed to different outputs due to control and interaction)
- Table that summarizes the trade-off for each strategy
- Say that in this study, we will do the control in the frame of the struts for simplicity (even though control in the cartesian frame was also tested)
** Active Damping with Decentralized IFF
<<ssec:nhexa_control_iff >>
Guaranteed stability: [[cite:&preumont08_trans_zeros_struc_contr_with ]]
- [ ] I think there is another paper about that
For decentralized control: "MIMO root locus" can be used to estimate the damping / optimal gain
Poles and converging towards /transmission zeros/
How to optimize the added damping to all modes?
- [ ] Add some papers citations
Compute:
- [ ] Plant dynamics
- [ ] Root Locus
** MIMO High-Authority Control - Low-Authority Control
<<ssec:nhexa_control_hac_lac >>
Compute:
- [ ] compare open-loop and damped plant (outputs are the encoders)
- [ ] Implement decentralized control?
- [ ] Check stability:
- Characteristic Loci: Eigenvalues of $G(j\omega)$ plotted in the complex plane
- Generalized Nyquist Criterion: If $G(s)$ has $p_0$ unstable poles, then the closed-loop system with return ratio $kG(s)$ is stable if and only if the characteristic loci of $kG(s)$, taken together, encircle the point $-1$, $p_0$ times anti-clockwise, assuming there are no hidden modes
- [ ] Show some performance metric? For instance compliance?
** Conclusion
:PROPERTIES:
:UNNUMBERED: t
:END:
2024-03-19 15:13:18 +01:00
* Conclusion
2025-02-05 17:47:37 +01:00
:PROPERTIES:
:UNNUMBERED: t
:END:
<<sec:nhexa_conclusion >>
- Configurable Stewart platform model
- Will be included in the multi-body model of the micro-station => nass multi body model
- Control: complex problem, try to use simplest architecture
2024-03-19 15:13:18 +01:00
* Bibliography :ignore:
#+latex : \printbibliography[heading=bibintoc,title={Bibliography}]
* Helping Functions :noexport:
** Initialize Path
#+NAME : m-init-path
#+BEGIN_SRC matlab
addpath('./matlab/ '); % Path for scripts
2025-02-07 18:00:49 +01:00
%% Path for functions, data and scripts
addpath('./matlab/mat/ '); % Path for Computed FRF
addpath('./matlab/src/ '); % Path for functions
addpath('./matlab/subsystems/ '); % Path for Subsystems Simulink files
%% Data directory
data_dir = './matlab/mat/ '
2024-03-19 15:13:18 +01:00
#+END_SRC
#+NAME : m-init-path-tangle
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
2025-02-07 18:00:49 +01:00
addpath('./mat/ '); % Path for Data
addpath('./src/ '); % Path for functions
addpath('./subsystems/ '); % Path for Subsystems Simulink files
%% Data directory
data_dir = './mat/ ';
2024-03-19 15:13:18 +01:00
#+END_SRC
2025-02-07 18:00:49 +01:00
** Initialize Simscape Model
#+NAME : m-init-simscape
#+begin_src matlab
% Simulink Model name
mdl = 'nano_hexapod_model';
#+end_src
2024-03-19 15:13:18 +01:00
** Initialize other elements
#+NAME : m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
2025-02-07 18:00:49 +01:00
%% Frequency Vector
freqs = logspace(0, 3, 1000);
2024-03-19 15:13:18 +01:00
#+END_SRC
2025-02-07 18:00:49 +01:00
* Matlab Functions :noexport:
** =initializeLoggingConfiguration=: Logging Configuration
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
*** Function description
#+begin_src matlab
function [] = initializeLoggingConfiguration(args)
#+end_src
*** Optional Parameters
#+begin_src matlab
arguments
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
end
#+end_src
*** Structure initialization
#+begin_src matlab
conf_log = struct();
#+end_src
*** Add Type
#+begin_src matlab
switch args.log
case 'none'
conf_log.type = 0;
case 'all'
conf_log.type = 1;
case 'forces'
conf_log.type = 2;
end
#+end_src
*** Sampling Time
#+begin_src matlab
conf_log.Ts = args.Ts;
#+end_src
*** Save the Structure
#+begin_src matlab
if exist('./mat', 'dir')
if exist('./mat/nano_hexapod_model_conf_log.mat', 'file')
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
else
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nano_hexapod_model_conf_log.mat', 'file')
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
else
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log');
end
end
#+end_src
** =initializeController=: Initialize Controller
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
function [] = initializeController(args)
arguments
args.type char {mustBeMember(args.type,{'open-loop', 'iff', 'dvf', 'hac-dvf', 'ref-track-L', 'ref-track-iff-L', 'cascade-hac-lac', 'hac-iff', 'stabilizing'})} = 'open-loop'
end
controller = struct();
switch args.type
case 'open-loop'
controller.type = 1;
controller.name = 'Open-Loop';
case 'dvf'
controller.type = 2;
controller.name = 'Decentralized Direct Velocity Feedback';
case 'iff'
controller.type = 3;
controller.name = 'Decentralized Integral Force Feedback';
case 'hac-dvf'
controller.type = 4;
controller.name = 'HAC-DVF';
case 'ref-track-L'
controller.type = 5;
controller.name = 'Reference Tracking in the frame of the legs';
case 'ref-track-iff-L'
controller.type = 6;
controller.name = 'Reference Tracking in the frame of the legs + IFF';
case 'cascade-hac-lac'
controller.type = 7;
controller.name = 'Cascade Control + HAC-LAC';
case 'hac-iff'
controller.type = 8;
controller.name = 'HAC-IFF';
case 'stabilizing'
controller.type = 9;
controller.name = 'Stabilizing Controller';
end
if exist('./mat', 'dir')
save('mat/nano_hexapod_model_controller.mat', 'controller');
elseif exist('./matlab', 'dir')
save('matlab/mat/nano_hexapod_model_controller.mat', 'controller');
end
end
#+end_src
** =initializeSample=: Sample
#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no
function [sample] = initializeSample(args)
arguments
args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none'
args.H (1,1) double {mustBeNumeric, mustBePositive} = 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 = args.H;
sample.R = args.R;
sample.m = args.m;
end
if exist('./mat', 'dir')
if exist('./mat/nano_hexapod.mat', 'file')
save('mat/nano_hexapod.mat', 'sample', '-append');
else
save('mat/nano_hexapod.mat', 'sample');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nano_hexapod.mat', 'file')
save('matlab/mat/nano_hexapod.mat', 'sample', '-append');
else
save('matlab/mat/nano_hexapod.mat', 'sample');
end
end
end
#+end_src
** Stewart platform
*** =initializeSimplifiedNanoHexapod=: Nano Hexapod
#+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
arguments
%% initializeFramesPositions
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
%% generateGeneralConfiguration
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of fixed joints [m]
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 20e-3 % Height of mobile joints [m]
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
%% Actuators
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 380000
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 5
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
%% initializeCylindricalPlatforms
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
%% initializeCylindricalStruts
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
%% Bottom and Top Flexible Joints
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
%% inverseKinematics
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
stewart = initializeStewartPlatform();
stewart = initializeFramesPositions(stewart, ...
'H', args.H, ...
'MO_B', args.MO_B);
stewart = generateGeneralConfiguration(stewart, ...
'FH', args.FH, ...
'FR', args.FR, ...
'FTh', args.FTh, ...
'MH', args.MH, ...
'MR', args.MR, ...
'MTh', args.MTh);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, ...
'type', args.actuator_type, ...
'k', args.actuator_k, ...
'ke', args.actuator_ke, ...
'ka', args.actuator_ka, ...
'c', args.actuator_c, ...
'ce', args.actuator_ce, ...
'ca', args.actuator_ca);
stewart = initializeJointDynamics(stewart, ...
'type_F', args.flex_type_F, ...
'type_M', args.flex_type_M, ...
'Kf_M', args.Kf_M, ...
'Cf_M', args.Cf_M, ...
'Kt_M', args.Kt_M, ...
'Ct_M', args.Ct_M, ...
'Kf_F', args.Kf_F, ...
'Cf_F', args.Cf_F, ...
'Kt_F', args.Kt_F, ...
'Ct_F', args.Ct_F, ...
'Ka_F', args.Ka_F, ...
'Ca_F', args.Ca_F, ...
'Kr_F', args.Kr_F, ...
'Cr_F', args.Cr_F, ...
'Ka_M', args.Ka_M, ...
'Ca_M', args.Ca_M, ...
'Kr_M', args.Kr_M, ...
'Cr_M', args.Cr_M);
stewart = initializeCylindricalPlatforms(stewart, ...
'Fpm', args.Fpm, ...
'Fph', args.Fph, ...
'Fpr', args.Fpr, ...
'Mpm', args.Mpm, ...
'Mph', args.Mph, ...
'Mpr', args.Mpr);
stewart = initializeCylindricalStruts(stewart, ...
'Fsm', args.Fsm, ...
'Fsh', args.Fsh, ...
'Fsr', args.Fsr, ...
'Msm', args.Msm, ...
'Msh', args.Msh, ...
'Msr', args.Msr);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart, ...
'AP', args.AP, ...
'ARB', args.ARB);
nano_hexapod = stewart;
if exist('./mat', 'dir')
if exist('./mat/nano_hexapod.mat', 'file')
save('mat/nano_hexapod.mat', 'nano_hexapod', '-append');
else
save('mat/nano_hexapod.mat', 'nano_hexapod');
end
elseif exist('./matlab', 'dir')
if exist('./matlab/mat/nano_hexapod.mat', 'file')
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod', '-append');
else
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod');
end
end
end
#+end_src
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
#+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStewartPlatform()
% initializeStewartPlatform - Initialize the stewart structure
%
% Syntax: [stewart] = initializeStewartPlatform(args)
%
% Outputs:
% - stewart - A structure with the following sub-structures:
% - platform_F -
% - platform_M -
% - joints_F -
% - joints_M -
% - struts_F -
% - struts_M -
% - actuators -
% - geometry -
% - properties -
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints_F = struct();
stewart.joints_M = struct();
stewart.struts_F = struct();
stewart.struts_M = struct();
stewart.actuators = struct();
stewart.sensors = struct();
stewart.sensors.inertial = struct();
stewart.sensors.force = struct();
stewart.sensors.relative = struct();
stewart.geometry = struct();
stewart.kinematics = struct();
end
#+end_src
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
#+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no
function [stewart] = initializeFramesPositions(stewart, args)
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
%
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
%
% Inputs:
% - args - Can have the following fields:
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
%
% Outputs:
% - stewart - A structure with the following fields:
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
arguments
stewart
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
end
H = args.H; % Total Height of the Stewart Platform [m]
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
stewart.geometry.H = H;
stewart.geometry.FO_M = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;
end
#+end_src
*** =generateGeneralConfiguration=: Generate a Very General Configuration
#+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no
function [stewart] = generateGeneralConfiguration(stewart, args)
% generateGeneralConfiguration - Generate a Very General Configuration
%
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
%
% Inputs:
% - args - Can have the following fields:
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
arguments
stewart
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
end
Fa = zeros(3,6);
Mb = zeros(3,6);
for i = 1:6
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
end
stewart.platform_F.Fa = Fa;
stewart.platform_M.Mb = Mb;
end
#+end_src
*** =computeJointsPose=: Compute the Pose of the Joints
#+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no
function [stewart] = computeJointsPose(stewart)
% computeJointsPose -
%
% Syntax: [stewart] = computeJointsPose(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
%
% Outputs:
% - stewart - A structure with the following added fields
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
% - geometry.l [6x1] - The i'th element is the initial length of strut i
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
FO_M = stewart.geometry.FO_M;
Aa = Fa - repmat(FO_A, [1, 6]);
Bb = Mb - repmat(MO_B, [1, 6]);
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
l = vecnorm(Ab - Aa)';
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
FRa = zeros(3,3,6);
MRb = zeros(3,3,6);
for i = 1:6
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
end
stewart.geometry.Aa = Aa;
stewart.geometry.Ab = Ab;
stewart.geometry.Ba = Ba;
stewart.geometry.Bb = Bb;
stewart.geometry.As = As;
stewart.geometry.Bs = Bs;
stewart.geometry.l = l;
stewart.struts_F.l = l/2;
stewart.struts_M.l = l/2;
stewart.platform_F.FRa = FRa;
stewart.platform_M.MRb = MRb;
end
#+end_src
*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
#+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no
function [stewart] = initializeCylindricalPlatforms(stewart, args)
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
%
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
%
% Inputs:
% - args - Structure with the following fields:
% - Fpm [1x1] - Fixed Platform Mass [kg]
% - Fph [1x1] - Fixed Platform Height [m]
% - Fpr [1x1] - Fixed Platform Radius [m]
% - Mpm [1x1] - Mobile Platform Mass [kg]
% - Mph [1x1] - Mobile Platform Height [m]
% - Mpr [1x1] - Mobile Platform Radius [m]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - platform_F [struct] - structure with the following fields:
% - type = 1
% - M [1x1] - Fixed Platform Mass [kg]
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
% - H [1x1] - Fixed Platform Height [m]
% - R [1x1] - Fixed Platform Radius [m]
% - platform_M [struct] - structure with the following fields:
% - M [1x1] - Mobile Platform Mass [kg]
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
% - H [1x1] - Mobile Platform Height [m]
% - R [1x1] - Mobile Platform Radius [m]
arguments
stewart
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
end
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
1/2 *args.Fpm * args.Fpr^2]);
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
1/2 *args.Mpm * args.Mpr^2]);
stewart.platform_F.type = 1;
stewart.platform_F.I = I_F;
stewart.platform_F.M = args.Fpm;
stewart.platform_F.R = args.Fpr;
stewart.platform_F.H = args.Fph;
stewart.platform_M.type = 1;
stewart.platform_M.I = I_M;
stewart.platform_M.M = args.Mpm;
stewart.platform_M.R = args.Mpr;
stewart.platform_M.H = args.Mph;
end
#+end_src
*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
#+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no
function [stewart] = initializeCylindricalStruts(stewart, args)
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
%
% Syntax: [stewart] = initializeCylindricalStruts(args)
%
% Inputs:
% - args - Structure with the following fields:
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - struts_F [struct] - structure with the following fields:
% - M [6x1] - Mass of the Fixed part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
% - struts_M [struct] - structure with the following fields:
% - M [6x1] - Mass of the Mobile part of the struts [kg]
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
arguments
stewart
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
end
stewart.struts_M.type = 1;
stewart.struts_M.M = args.Msm;
stewart.struts_M.R = args.Msr;
stewart.struts_M.H = args.Msh;
stewart.struts_F.type = 1;
stewart.struts_F.M = args.Fsm;
stewart.struts_F.R = args.Fsr;
stewart.struts_F.H = args.Fsh;
end
#+end_src
*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
#+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStrutDynamics(stewart, args)
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
%
% Syntax: [stewart] = initializeStrutDynamics(args)
%
% Inputs:
% - args - Structure with the following fields:
% - K [6x1] - Stiffness of each strut [N/m]
% - C [6x1] - Damping of each strut [N/(m/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.type = 1
% - actuators.K [6x1] - Stiffness of each strut [N/m]
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
arguments
stewart
args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
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;
stewart.actuators.ka = args.ka;
stewart.actuators.ca = args.ca;
stewart.actuators.ke = args.ke;
stewart.actuators.ce = args.ce;
stewart.actuators.F_gain = args.F_gain;
stewart.actuators.ma = args.ma;
stewart.actuators.me = args.me;
end
#+end_src
*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeJointDynamics(stewart, args)
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
%
% Syntax: [stewart] = initializeJointDynamics(args)
%
% Inputs:
% - args - Structure with the following fields:
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - stewart.joints_F and stewart.joints_M:
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
arguments
stewart
args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.K_M double {mustBeNumeric} = zeros(6,6)
args.M_M double {mustBeNumeric} = zeros(6,6)
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
args.xi_M double {mustBeNumeric} = 0.1
args.step_file_M char {} = ''
args.K_F double {mustBeNumeric} = zeros(6,6)
args.M_F double {mustBeNumeric} = zeros(6,6)
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
args.xi_F double {mustBeNumeric} = 0.1
args.step_file_F char {} = ''
end
switch args.type_F
case '2dof'
stewart.joints_F.type = 1;
case '3dof'
stewart.joints_F.type = 2;
case '4dof'
stewart.joints_F.type = 3;
case '6dof'
stewart.joints_F.type = 4;
case 'flexible'
stewart.joints_F.type = 5;
otherwise
error("joints_F are not correctly defined")
end
switch args.type_M
case '2dof'
stewart.joints_M.type = 1;
case '3dof'
stewart.joints_M.type = 2;
case '4dof'
stewart.joints_M.type = 3;
case '6dof'
stewart.joints_M.type = 4;
case 'flexible'
stewart.joints_M.type = 5;
otherwise
error("joints_M are not correctly defined")
end
stewart.joints_M.Ka = args.Ka_M;
stewart.joints_M.Kr = args.Kr_M;
stewart.joints_F.Ka = args.Ka_F;
stewart.joints_F.Kr = args.Kr_F;
stewart.joints_M.Ca = args.Ca_M;
stewart.joints_M.Cr = args.Cr_M;
stewart.joints_F.Ca = args.Ca_F;
stewart.joints_F.Cr = args.Cr_F;
stewart.joints_M.Kf = args.Kf_M;
stewart.joints_M.Kt = args.Kt_M;
stewart.joints_F.Kf = args.Kf_F;
stewart.joints_F.Kt = args.Kt_F;
stewart.joints_M.Cf = args.Cf_M;
stewart.joints_M.Ct = args.Ct_M;
stewart.joints_F.Cf = args.Cf_F;
stewart.joints_F.Ct = args.Ct_F;
stewart.joints_F.M = args.M_F;
stewart.joints_F.K = args.K_F;
stewart.joints_F.n_xyz = args.n_xyz_F;
stewart.joints_F.xi = args.xi_F;
stewart.joints_F.xi = args.xi_F;
stewart.joints_F.step_file = args.step_file_F;
stewart.joints_M.M = args.M_M;
stewart.joints_M.K = args.K_M;
stewart.joints_M.n_xyz = args.n_xyz_M;
stewart.joints_M.xi = args.xi_M;
stewart.joints_M.step_file = args.step_file_M;
end
#+end_src
*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
#+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no
function [stewart] = initializeStewartPose(stewart, args)
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
% It uses the inverse kinematic
%
% Syntax: [stewart] = initializeStewartPose(stewart, args)
%
% Inputs:
% - stewart - A structure with the following fields
% - Aa [3x6] - The positions ai expressed in {A}
% - Bb [3x6] - The positions bi expressed in {B}
% - args - Can have the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
stewart.actuators.Leq = dLi;
end
#+end_src
*** =computeJacobian=: Compute the Jacobian Matrix
#+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no
function [stewart] = computeJacobian(stewart)
% computeJacobian -
%
% Syntax: [stewart] = computeJacobian(stewart)
%
% Inputs:
% - stewart - With at least the following fields:
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
% - actuators.K [6x1] - Total stiffness of the actuators
%
% Outputs:
% - stewart - With the 3 added field:
% - geometry.J [6x6] - The Jacobian Matrix
% - geometry.K [6x6] - The Stiffness Matrix
% - geometry.C [6x6] - The Compliance Matrix
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
As = stewart.geometry.As;
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
Ab = stewart.geometry.Ab;
assert(isfield(stewart.actuators, 'k'), 'stewart.actuators should have attribute k')
Ki = stewart.actuators.k;
J = [As' , cross(Ab, As)'];
K = J'*diag(Ki)*J;
C = inv(K);
stewart.geometry.J = J;
stewart.geometry.K = K;
stewart.geometry.C = C;
end
#+end_src
*** =inverseKinematics=: Compute Inverse Kinematics
#+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no
function [Li, dLi] = inverseKinematics(stewart, args)
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
%
% Syntax: [stewart] = inverseKinematics(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - geometry.Aa [3x6] - The positions ai expressed in {A}
% - geometry.Bb [3x6] - The positions bi expressed in {B}
% - geometry.l [6x1] - Length of each strut
% - args - Can have the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
%
% Outputs:
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
end
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
Aa = stewart.geometry.Aa;
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
Bb = stewart.geometry.Bb;
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
l = stewart.geometry.l;
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'* (args.ARB*Bb))' - diag(2* (args.ARB*Bb)'*Aa));
dLi = Li-l;
end
#+end_src
*** =displayArchitecture=: 3D plot of the Stewart platform architecture
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/displayArchitecture.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:displayArchitecture >>
This Matlab function is accessible [[file:../src/displayArchitecture.m ][here ]].
**** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = displayArchitecture(stewart, args)
% displayArchitecture - 3D plot of the Stewart platform architecture
%
% Syntax: [] = displayArchitecture(args)
%
% Inputs:
% - stewart
% - args - Structure with the following fields:
% - AP [3x1] - The wanted position of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
% - F_color [color] - Color used for the Fixed elements
% - M_color [color] - Color used for the Mobile elements
% - L_color [color] - Color used for the Legs elements
% - frames [true/false] - Display the Frames
% - legs [true/false] - Display the Legs
% - joints [true/false] - Display the Joints
% - labels [true/false] - Display the Labels
% - platforms [true/false] - Display the Platforms
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
%
% Outputs:
#+end_src
**** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
args.F_color = [0 0.4470 0.7410]
args.M_color = [0.8500 0.3250 0.0980]
args.L_color = [0 0 0]
args.frames logical {mustBeNumericOrLogical} = true
args.legs logical {mustBeNumericOrLogical} = true
args.joints logical {mustBeNumericOrLogical} = true
args.labels logical {mustBeNumericOrLogical} = true
args.platforms logical {mustBeNumericOrLogical} = true
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
end
#+end_src
**** Check the =stewart= structure elements
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
FO_A = stewart.platform_F.FO_A;
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
MO_B = stewart.platform_M.MO_B;
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
H = stewart.geometry.H;
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
Fa = stewart.platform_F.Fa;
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
Mb = stewart.platform_M.Mb;
#+end_src
**** Figure Creation, Frames and Homogeneous transformations
:PROPERTIES:
:UNNUMBERED: t
:END:
The reference frame of the 3d plot corresponds to the frame $\{F\}$.
#+begin_src matlab
if ~strcmp(args.views, 'all')
figure;
else
f = figure('visible', 'off');
end
hold on;
#+end_src
We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
#+begin_src matlab
FTa = [eye(3), FO_A; ...
zeros(1,3), 1];
ATb = [args.ARB, args.AP; ...
zeros(1,3), 1];
BTm = [eye(3), -MO_B; ...
zeros(1,3), 1];
FTm = FTa*ATb*BTm;
#+end_src
Let's define a parameter that define the length of the unit vectors used to display the frames.
#+begin_src matlab
d_unit_vector = H/4;
#+end_src
Let's define a parameter used to position the labels with respect to the center of the element.
#+begin_src matlab
d_label = H/20;
#+end_src
**** Fixed Base elements
:PROPERTIES:
:UNNUMBERED: t
:END:
Let's first plot the frame $\{F\}$.
#+begin_src matlab
Ff = [0, 0, 0];
if args.frames
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Ff(1) + d_label, ...
Ff(2) + d_label, ...
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
end
end
#+end_src
Now plot the frame $\{A\}$ fixed to the Base.
#+begin_src matlab
if args.frames
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(FO_A(1) + d_label, ...
FO_A(2) + d_label, ...
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
end
end
#+end_src
Let's then plot the circle corresponding to the shape of the Fixed base.
#+begin_src matlab
if args.platforms && stewart.platform_F.type == 1
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
center = [0; 0; 0]; % Center of the circle
radius = stewart.platform_F.R; % Radius of the circle [m]
points = center*ones(1, length(theta)) + radius* (v(:,1)*cos(theta) + v(:,2)*sin(theta));
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', args.F_color);
end
#+end_src
Let's now plot the position and labels of the Fixed Joints
#+begin_src matlab
if args.joints
scatter3(Fa(1,:), ...
Fa(2,:), ...
Fa(3,:), 'MarkerEdgeColor', args.F_color);
if args.labels
for i = 1:size(Fa,2)
text(Fa(1,i) + d_label, ...
Fa(2,i), ...
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
end
end
end
#+end_src
**** Mobile Platform elements
:PROPERTIES:
:UNNUMBERED: t
:END:
Plot the frame $\{M\}$.
#+begin_src matlab
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
if args.frames
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(Fm(1) + d_label, ...
Fm(2) + d_label, ...
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
end
end
#+end_src
Plot the frame $\{B\}$.
#+begin_src matlab
FB = FO_A + args.AP;
if args.frames
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(FB(1) - d_label, ...
FB(2) + d_label, ...
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
end
end
#+end_src
Let's then plot the circle corresponding to the shape of the Mobile platform.
#+begin_src matlab
if args.platforms && stewart.platform_M.type == 1
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
center = Fm(1:3); % Center of the circle
radius = stewart.platform_M.R; % Radius of the circle [m]
points = center*ones(1, length(theta)) + radius* (v(:,1)*cos(theta) + v(:,2)*sin(theta));
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', args.M_color);
end
#+end_src
Plot the position and labels of the rotation joints fixed to the mobile platform.
#+begin_src matlab
if args.joints
Fb = FTm*[Mb;ones(1,6)];
scatter3(Fb(1,:), ...
Fb(2,:), ...
Fb(3,:), 'MarkerEdgeColor', args.M_color);
if args.labels
for i = 1:size(Fb,2)
text(Fb(1,i) + d_label, ...
Fb(2,i), ...
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
end
end
end
#+end_src
**** Legs
:PROPERTIES:
:UNNUMBERED: t
:END:
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
#+begin_src matlab
if args.legs
for i = 1:6
plot3([Fa(1,i), Fb(1,i)], ...
[Fa(2,i), Fb(2,i)], ...
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
if args.labels
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
(Fa(2,i)+Fb(2,i))/2, ...
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
end
end
end
#+end_src
**** Figure parameters
#+begin_src matlab
switch args.views
case 'default'
view([1 -0.6 0.4]);
case 'xy'
view([0 0 1]);
case 'xz'
view([0 -1 0]);
case 'yz'
view([1 0 0]);
end
axis equal;
axis off;
#+end_src
**** Subplots
#+begin_src matlab
if strcmp(args.views, 'all')
hAx = findobj('type', 'axes');
figure;
s1 = subplot(2,2,1);
copyobj(get(hAx(1), 'Children'), s1);
view([0 0 1]);
axis equal;
axis off;
title('Top')
s2 = subplot(2,2,2);
copyobj(get(hAx(1), 'Children'), s2);
view([1 -0.6 0.4]);
axis equal;
axis off;
s3 = subplot(2,2,3);
copyobj(get(hAx(1), 'Children'), s3);
view([1 0 0]);
axis equal;
axis off;
title('Front')
s4 = subplot(2,2,4);
copyobj(get(hAx(1), 'Children'), s4);
view([0 -1 0]);
axis equal;
axis off;
title('Side')
close(f);
end
#+end_src
*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
:PROPERTIES:
:header-args:matlab+: :tangle matlab/src/describeStewartPlatform.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:describeStewartPlatform >>
This Matlab function is accessible [[file:../src/describeStewartPlatform.m ][here ]].
**** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [] = describeStewartPlatform(stewart)
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
%
% Syntax: [] = describeStewartPlatform(args)
%
% Inputs:
% - stewart
%
% Outputs:
#+end_src
**** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
stewart
end
#+end_src
**** Geometry
#+begin_src matlab
fprintf('GEOMETRY:\n')
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
if stewart.platform_M.MO_B(3) > 0
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
else
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
end
fprintf('- The initial length of the struts are:\n')
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
fprintf('\n')
#+end_src
**** Actuators
#+begin_src matlab
fprintf('ACTUATORS:\n')
if stewart.actuators.type == 1
fprintf('- The actuators are modelled as 1DoF.\n')
fprintf('- The Stiffness and Damping of each actuators is:\n')
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.K(1), stewart.actuators.C(1))
elseif stewart.actuators.type == 2
fprintf('- The actuators are modelled as 2DoF (APA).\n')
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.Ka(1), stewart.actuators.Ca(1))
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.Kr(1), stewart.actuators.Cr(1))
elseif stewart.actuators.type == 3
fprintf('- The actuators are modelled with a flexible element (FEM).\n')
end
fprintf('\n')
#+end_src
**** Joints
#+begin_src matlab
fprintf('JOINTS:\n')
#+end_src
Type of the joints on the fixed base.
#+begin_src matlab
switch stewart.joints_F.type
case 1
fprintf('- The joints on the fixed based are universal joints (2DoF)\n')
case 2
fprintf('- The joints on the fixed based are spherical joints (3DoF)\n')
end
#+end_src
Type of the joints on the mobile platform.
#+begin_src matlab
switch stewart.joints_M.type
case 1
fprintf('- The joints on the mobile based are universal joints (2DoF)\n')
case 2
fprintf('- The joints on the mobile based are spherical joints (3DoF)\n')
end
#+end_src
Position of the fixed joints
#+begin_src matlab
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
#+end_src
Position of the mobile joints
#+begin_src matlab
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
fprintf('\n')
#+end_src
**** Kinematics
#+begin_src matlab
fprintf('KINEMATICS:\n')
if isfield(stewart.kinematics, 'K')
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
end
if isfield(stewart.kinematics, 'C')
fprintf('- The Damping matrix C is (in [m/N]):\n')
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
end
#+end_src
* Footnotes
[fn:1]Different architecture exists, typically referred as "6-SPS" (Spherical, Prismatic, Spherical) or "6-UPS" (Universal, Prismatic, Spherical)
