tdehaeze/delta-robot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
7f107550e7cf0af0e9455a63d34757ddd1e5fe33
delta-robot/delta-robot.org
Thomas Dehaeze 7f107550e7 Initial commit
2025-12-02 14:33:46 +01:00

2639 lines
99 KiB
Org Mode
Raw Blame History

#+TITLE: Delta Robot
: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]
#+LATEX_HEADER: \input{preamble.tex}
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
#+BIND: org-latex-bib-compiler "biber"
#+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 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
* The Delta Robot Kinematics :ignore:
<<sec:delta_robot_introduction>>
** Introduction :ignore:
** 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-simscape>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
** Geometry
The Delta Robot geometry is defined as shown in Figure ref:fig:delta_robot_schematic.
The geometry is fully defined by three parameters:
- =d=: Cube's size (i.e., the length of the cube edge)
- =a=: Distance from cube's vertex to top flexible joint
- =L=: Distance between two flexible joints (i.e., the length of the struts)
#+name: fig:delta_robot_schematic
#+caption: Schematic of the Delta Robot
[[file:figs/delta_robot_schematic.png]]
Several frames are defined:
- $\{C\}$: Cube's center
- $\{M\}$: Frame attached to the mobile platform, and located at the height of the top flexible joints
- $\{F\}$: Frame attached to the fixed platform, and located at the height of the bottom flexible joints
Several points are defined:
- $c_i$: vertices of the cubes which are relevant for the Delta Robot
- $b_i$: location of the top flexible joints
- $a_i$: location of the bottom flexible joints
- $\hat{s}_i$: unit vector aligned with the struts
Static properties:
- All top and bottom flexible joints are identical.
The following properties can be specified:
- $k_a$: Axial stiffness
- $k_r$: Radial stiffness
- $k_b$: Bending stiffness
- $k_t$: Torsion stiffness
- The guiding mechanism of the actuator is here supposed to be perfect (i.e. 1dof system without any stiffness)
- The Actuator is modelled as a 1DoF or 2DoF (good to model APA):
- The custom developed APA has an axial stiffness of $1.3\,N/\mu m$
Dynamical properties:
- Top platform inertia:
It has a mass of ~300g
- Payloads: payloads can weight up to 1kg
Let's initialize a Delta Robot architecture, and plot the obtained geometry (Figures ref:fig:delta_robot_architecture and ref:fig:delta_robot_architecture_top).
#+begin_src matlab
%% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
L = 50e-3; % Length of the struts [m]
%% Initialize the Delta Robot
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot);
delta_robot = initializeJointDynamics(delta_robot);
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
#+end_src
#+begin_src matlab :exports none :results none
%% Delta Robot Architecture
displayArchitecture(delta_robot, 'labels', true, 'frames', true);
plotCube(delta_robot, 'Hc', d/sqrt(3), 'FOc', delta_robot.geometry.H+delta_robot.platform_M.MO_B(3), 'color', [0,0,0,0.2], 'link_to_struts', false);
view([70, 30]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_architecture.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_architecture
#+caption: Delta Robot Architecture
#+RESULTS:
[[file:figs/delta_robot_architecture.png]]
#+begin_src matlab :tangle no :exports results :results file replace
view([0, 90]);
exportFig('figs/delta_robot_architecture_top.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_architecture_top
#+caption: Delta Robot Architecture - Top View
#+RESULTS:
[[file:figs/delta_robot_architecture_top.png]]
** Kinematics: Jacobian Matrix and Mobility
Jacobian matrix between actuator displacement and top platform displacement.
There are three actuators in the following directions $\hat{s}_1$, $\hat{s}_2$ and $\hat{s}_3$;
\begin{equation}\label{eq:detail_kinematics_cubic_s}
\hat{\bm{s}}_1 = \begin{bmatrix} \frac{-1}{\sqrt{6}} \\ \frac{-1}{\sqrt{2}} \\ \frac{1}{\sqrt{3}} \end{bmatrix} \quad
\hat{\bm{s}}_2 = \begin{bmatrix} \frac{\sqrt{2}}{\sqrt{3}} \\ 0 \\ \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}
\end{equation}
\begin{equation}
\bm{J} = \begin{bmatrix}
\hat{\bm{s}}_1^T \\ \hat{\bm{s}}_2^T \\ \hat{\bm{s}}_3^T
\end{bmatrix}
\end{equation}
#+begin_src matlab
s1 = delta_robot.geometry.As(:,1);
s2 = delta_robot.geometry.As(:,3);
s3 = delta_robot.geometry.As(:,5);
J = [s1' ; s2' ; s3']
#+end_src
\begin{equation}
d\mathcal{L} = J d\mathcal{L}
\end{equation}
\begin{equation}
d\mathcal{X} = J^{-1} d\mathcal{L}
\end{equation}
The achievable workspace is a cube whose edge length is equal to the actuator stroke.
#+begin_src matlab
L_max = 50e-6; % Maximum actuator stroke (+/-) [m]
thetas = linspace(0, pi, 100);
phis = linspace(0, 2*pi, 100);
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 = J*[Tx; Ty; Tz]; % dL required for 1m displacement in theta/phi direction
rs(i, j) = L_max/max(abs(dL));
end
end
#+end_src
#+begin_src matlab :exports none :results none
%% 3D workspace
figure('Renderer', 'painters');
hold on;
surf(X, Y, Z, 'FaceColor', 'none', 'LineWidth', 0.1, 'EdgeColor', [0, 0, 0]);
quiver3(0, 0, 0, 100, 0, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 100, 0, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
quiver3(0, 0, 0, 0, 0, 100, 'k', 'LineWidth', 2, 'MaxHeadSize', 0.9);
hold off;
axis equal;
grid off;
axis off;
view(55, 25);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_3d_workspace.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_3d_workspace
#+caption: 3D workspace
#+RESULTS:
[[file:figs/delta_robot_3d_workspace.png]]
As most likely, the system will be used to perform YZ scans, it is interesting to see the mobility of the system in the ZY plane.
Depending on how the YZ plane is oriented (i.e., depending on the Rz angle of the delta robot with respect to the beam, defining the x direction), we get different mobility.
#+begin_src matlab
%% 2D mobility for different angles
L_max = 150e-6; % Maximum actuator stroke (+/-) [m]
thetas = linspace(0, 2*pi, 1001);
phis = linspace(0, pi/2, 101);
rs = zeros(length(thetas), length(phis));
for i = 1:length(thetas)
for j = 1:length(phis)
Tx = cos(thetas(i))*cos(phis(j));
Ty = cos(thetas(i))*sin(phis(j));
Tz = sin(thetas(i));
dL = J*[Tx; Ty; Tz]; % dL required for 1m displacement in theta/phi direction
rs(i, j) = L_max/max(abs(dL));
end
end
%% Get minimum square in the Y/Z plane
[~, i] = min(abs(thetas-pi/4));
L = 1/sqrt(2)*min(abs(rs(i,:)));
#+end_src
#+begin_src matlab :exports none :results none
%% 2D mobility for different orientations
figure;
hold on;
for i = 1:length(phis)
plot(1e6*rs(:,i).*cos(thetas'), 1e6*rs(:,i).*sin(thetas'), 'k-', 'HandleVisibility', 'off')
end
plot(1e6*[-L, L, L, -L, -L], 1e6*[-L, -L, L, L, -L], 'r-', 'DisplayName', sprintf('$\\pm %.0f \\mu m$', 1e6*L))
hold off;
axis equal;
xlabel('Y Displacemnt [$\mu$m]');
ylabel('Z Displacemnt [$\mu$m]');
xlim([-300, 300]);
ylim([-300, 300]);
legend()
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_2d_workspace.pdf', 'width', 'normal', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_2d_workspace
#+caption: 2D mobility for different orientations
#+RESULTS:
[[file:figs/delta_robot_2d_workspace.png]]
#+begin_src matlab
%% Get the orientation that gives the best YZ mobility
L_max = 150e-6; % Maximum actuator stroke (+/-) [m]
thetas = [pi/4, pi/4+pi/2, pi/4+pi, pi/4+3*pi/2];
phis = linspace(0, 2*pi, 1001);
rs = zeros(length(thetas), length(phis));
for i = 1:length(thetas)
for j = 1:length(phis)
Tx = cos(thetas(i))*cos(phis(j));
Ty = cos(thetas(i))*sin(phis(j));
Tz = sin(thetas(i));
dL = J*[Tx; Ty; Tz]; % dL required for 1m displacement in theta/phi direction
rs(i, j) = L_max/max(abs(dL));
end
end
[Lmax, imax] = max(min(rs));
Dmax = Lmax/sqrt(2); % Size of the cube
#+end_src
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Maximum YZ mobility for an angle of %.0f degrees, square with edge size of %.0f um', 180/pi*phis(imax), 1e6*Dmax);
#+end_src
#+RESULTS:
: Maximum YZ mobility for an angle of 270 degrees, square with edge size of 117 um
#+begin_src matlab :exports none :results none
thetas = linspace(0, 2*pi, 1001);
phi = phis(imax);
rs = zeros(length(thetas), 1);
for i = 1:length(thetas)
Tx = cos(thetas(i))*cos(phi);
Ty = cos(thetas(i))*sin(phi);
Tz = sin(thetas(i));
dL = J*[Tx; Ty; Tz]; % dL required for 1m displacement in theta/phi direction
rs(i) = L_max/max(abs(dL));
end
#+end_src
#+begin_src matlab :exports none :results none
%% 2D mobility for the optimal Rz angle
figure;
hold on;
plot(1e6*rs.*cos(thetas'), 1e6*rs.*sin(thetas'), 'k-', 'HandleVisibility', 'off')
plot(1e6*[-Dmax, Dmax, Dmax, -Dmax, -Dmax], 1e6*[-Dmax, -Dmax, Dmax, Dmax, -Dmax], 'r-', 'DisplayName', sprintf('$\\pm %.0f \\mu m$', 1e6*Dmax))
hold off;
axis equal;
xlabel('Y Displacemnt [$\mu$m]');
ylabel('Z Displacemnt [$\mu$m]');
xlim([-300, 300]);
ylim([-300, 300]);
legend()
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_2d_workspace_optimal.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_2d_workspace_optimal
#+caption: 2D mobility for the optimal Rz angle
#+RESULTS:
[[file:figs/delta_robot_2d_workspace_optimal.png]]
** Kinematics: Degrees of Freedom
In the perfect case (flexible joints having no stiffness in bending, and infinite stiffness in torsion and in the axial direction), the top platform is allowed to move only in the X, Y and Z directions while the three rotations are fixed.
In order to have some compliance in rotation, the flexible joints need to have some compliance in torsion *and* in the axial direction.
If only the torsional compliance is considered, or only the axial compliance, the top platform will still not be able to do any rotation.
This is shown below with the Simscape model:
Perfect Delta Robot:
- infinite axial stiffness
- infinite torsional stiffness
- no bending stiffness
It gives infinite stiffness in rotations, and a stiffness of $1\,N/\mu m$ in X, Y and Z directions (i.e. equal to the actuator stiffness).
#+begin_src matlab
%% Initialize the Delta Robot
% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
L = 50e-3; % Length of the struts [m]
% Actuator
k = 1e6; % [N/m]
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '2dof', 'type_M', '2dof');
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
% Sample on top
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', 50e-3, 'R', 20e-3);
#+end_src
#+begin_src matlab
%% Perfect Joints
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '2dof', 'type_M', '2dof');
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/6dof_metrology'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Run the linearization
G_perfect = linearize(mdl, io);
G_perfect.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_perfect.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
#+begin_src matlab :results value replace :exports results :tangle no
sprintf('Stiffness in X,Y and Z directions: %.1f N/um', 1e6*dcgain(G_perfect(1,1)))
#+end_src
#+RESULTS:
: Stiffness in X,Y and Z directions: 1.0 N/um
If we consider the torsion of the flexible joints:
- infinite axial stiffness
- finite torsional stiffness
- no bending stiffness
We get the same result.
#+begin_src matlab
%% Consider some torsional stiffness
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof', 'type_M', '3dof', 'kt', 100);
% Run the linearization
G_kt = linearize(mdl, io);
G_kt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_kt.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
#+begin_src matlab :results value replace :exports results :tangle no
dcgain(G_kt)
#+end_src
#+RESULTS:
| 1.00000000008084e-06 | 5.61161767954149e-17 | 1.71532908969278e-16 | -5.07840362381474e-20 | 6.66481298362927e-20 | -2.97485996797085e-20 |
| 9.44550093013109e-17 | 9.99999999981042e-07 | -1.31365181854596e-16 | 2.20736442292089e-20 | -8.27761658859861e-20 | -5.51841106039072e-20 |
| 1.5172479677791e-16 | 7.57916759433382e-18 | 9.9999999983089e-07 | 2.71965732539802e-20 | -5.0088900526952e-21 | 4.20708731166142e-20 |
| 3.7990974483635e-15 | -1.49195278352597e-15 | 1.09166051384021e-15 | -4.60992688885629e-28 | 4.46894514064728e-28 | -6.04202812027976e-29 |
| 1.92223678174906e-15 | 3.62267008376981e-15 | -5.42452104276465e-15 | 3.7737029650938e-28 | -2.99601307180469e-28 | -3.30297132145485e-28 |
| 3.79407058351761e-16 | -3.46944690994006e-17 | 9.05659407516431e-16 | -1.33615244419949e-28 | 6.31115609464934e-29 | -1.07596265815569e-29 |
If we consider the axial of the flexible joints:
- finite axial stiffness
- infinite torsional stiffness
- no bending stiffness
We get the same result.
#+begin_src matlab
%% Consider some axial stiffness
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '2dof_a', 'type_M', '2dof_a', 'ka', 100e6);
% Run the linearization
G_ka = linearize(mdl, io);
G_ka.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_ka.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
#+begin_src matlab :results value replace :exports results :tangle no
dcgain(G_ka)
#+end_src
#+RESULTS:
| 1.0100082922792e-06 | -8.23544084894984e-12 | -1.12982601885747e-11 | -1.30123733161505e-19 | 3.61079565098929e-21 | -2.56479299609534e-20 |
| -8.23691147427908e-12 | 1.01002426764947e-06 | 1.16488393394949e-11 | -9.17271935353013e-20 | -1.39093131659514e-20 | -8.92049690205949e-21 |
| -1.12984563170605e-11 | 1.1646579416975e-11 | 1.01001628184113e-06 | -1.2624013403036e-19 | -4.41063676650963e-22 | 2.01282903077792e-20 |
| 3.25566266033832e-16 | 7.02470264849632e-18 | 1.34879235410707e-17 | -4.42814771779206e-29 | 1.06332319737688e-30 | -8.04051214064698e-30 |
| -2.34669385520782e-15 | 1.03658500184429e-16 | 2.10026418149746e-15 | 3.04125606965025e-29 | -1.07347740796227e-29 | 1.00530401393662e-28 |
| -2.76788049700945e-16 | -3.49174266014038e-17 | 1.36253386200494e-15 | -1.31490052587614e-28 | -1.10493913866777e-30 | 3.45058758900066e-29 |
No we consider both finite torsional stiffness and finite axial stiffness.
In that case we get some compliance in rotation.
So it is a combination of axial and torsion stiffness that gives some rotational stiffness of the top platform.
#+begin_src matlab
%% Consider both the axial and torsional compliances of the joints
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '3dof_a', 'ka', 100e6, 'kt', 100);
% Run the linearization
G_ka_kt = linearize(mdl, io);
G_ka_kt.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G_ka_kt.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
#+begin_src matlab :results value replace :exports results :tangle no
dcgain(G_ka_kt)
#+end_src
#+RESULTS:
| 1.01000347398415e-06 | 1.08760660507374e-12 | 6.48998902114602e-12 | -1.68758827983914e-12 | 2.72734564017018e-12 | -1.13327901508316e-12 |
| -1.7269667100718e-13 | 1.01000152837256e-06 | 4.09018170284123e-13 | -9.80286569772779e-13 | -9.94629340226498e-13 | 8.3468855003368e-13 |
| 5.40707734744154e-12 | -1.46859567258244e-12 | 1.0099981133678e-06 | 3.34755457607493e-12 | -6.56764113866634e-12 | 1.26237249518737e-12 |
| 2.67935189951885e-13 | -6.64493555908236e-14 | 7.44661955503977e-13 | 1.59744415094004e-05 | -6.97585818773949e-13 | 1.44845764745933e-13 |
| 1.91880640934689e-14 | -1.31284160978049e-13 | -5.83827721443037e-13 | 4.97550111979747e-13 | 1.5974440506229e-05 | 8.27388125221276e-14 |
| 1.63302978397554e-14 | -6.29948151015302e-14 | 1.11798835985125e-13 | -4.74438363934587e-14 | -1.39110944875938e-13 | 3.99840072236264e-06 |
Therefore, to model some compliance of the top platform in rotation, both the axial compliance and the torsional compliance of the flexible joints should be considered.
** Kinematics: Number of modes
In the perfect condition (i.e. infinite stiffness in torsion and in compression of the flexible joints), the system has 6 states (i.e. 3 modes, one for each DoF: X, Y and Z).
When considering some compliance in torsion of the flexible joints, 12 states are added (one internal mode of the struts).
To remove these internal states (that might not be interesting but that could slow the simulations), one of the joint can have this torsional compliance while the other can have the torsional DoF constrained.
#+begin_src matlab
%% Verify that we get 6 states in the ideal case
% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
L = 50e-3; % Length of the struts [m]
% Actuator
k = 1e6; % [N/m]
% Initialize the Delta Robot
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '2dof', 'type_M', '3dof');
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
% Sample on top
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', 50e-3, 'R', 20e-3);
% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
#+end_src
#+begin_src matlab :results output replace :exports results :tangle no
size(G)
#+end_src
#+RESULTS:
: State-space model with 3 outputs, 3 inputs, and 6 states.
* Flexible Joint Design
<<sec:delta_robot_flexible_joints>>
** Introduction :ignore:
The goal is to extract specifications for the flexible joints of the six struts.
The two most critical flexible joints imperfections are:
- The axial stiffness, that should be high enough
- The bending stiffness, that should be low enough
The shear and torsional stiffnesses are not foreseen to be very problematic, but their impact will be evaluated.
First, the dynamics of a "perfect" Delta-Robot is identified (i.e. with perfect 2DoF rotational joints).
Then, the impact of the flexible joint's imperfections will be studied.
** 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-simscape>>
#+end_src
#+begin_src matlab :noweb yes
<<m-init-other>>
#+end_src
** Studied Geometry
The cube's edge length is equal to 50mm, the distance between cube's vertices and top joints is 20mm and the length of the struts (i.e. the distance between the two flexible joints of the same strut) is 50mm.
The actuator stiffness is $1\,N/\mu m$.
The obtained geometry is shown in Figure ref:fig:delta_robot_studied_geometry.
#+begin_src matlab
%% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
L = 50e-3; % Length of the struts [m]
%% Actuator
k = 1e6; % [N/m]
#+end_src
#+begin_src matlab
%% Initialize the Delta Robot
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof', 'type_M', '3dof');
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
#+end_src
#+begin_src matlab :exports none :results none
%% Geometry of the studied Delta Robot
displayArchitecture(delta_robot, 'labels', false, 'frames', false);
plotCube(delta_robot, 'Hc', d/sqrt(3), 'FOc', delta_robot.geometry.H+delta_robot.platform_M.MO_B(3), 'color', [0,0,0,0.2], 'link_to_struts', false);
view([70, 30]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_studied_geometry.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_studied_geometry
#+caption: Geometry of the studied Delta Robot
#+RESULTS:
[[file:figs/delta_robot_studied_geometry.png]]
#+begin_src matlab
%% Sample on top
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', 50e-3, 'R', 20e-3);
%% Input/Output definition of the Simscape model
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
#+end_src
The dynamics is first identified in perfect conditions (infinite axial stiffness of the joints, zero bending stiffness).
We get =State-space model with 3 outputs, 3 inputs, and 6 states.=
We get a perfectly decoupled system, with three identical modes in the X, Y and Z directions.
The dynamics is shown in Figure ref:fig:delta_robot_dynamics_perfect.
#+begin_src matlab :exports none :results none
%% Dynamics of the delta robot with perfect joints
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'DisplayName', '$D_i/F_j$');
for i = 1:2
for j = i+1:3
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
'DisplayName', '$D_1/F_1$');
plot(freqs, abs(squeeze(freqresp(G(2, 2), freqs, 'Hz'))), 'color', colors(2,:), ...
'DisplayName', '$D_2/F_2$');
plot(freqs, abs(squeeze(freqresp(G(3, 3), freqs, 'Hz'))), 'color', colors(3,:), ...
'DisplayName', '$D_3/F_3$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_dynamics_perfect.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_dynamics_perfect
#+caption: Dynamics of the delta robot with perfect joints
#+RESULTS:
[[file:figs/delta_robot_dynamics_perfect.png]]
** Stiffness seen by the actuator
Because the flexible joints will have some bending stiffness, the actuator in one direction will "see" some stiffness due to the struts in the other directions.
This will limit its effective stroke.
We want this parallel stiffness to be much smaller than the stiffness of the actuator.
The parallel stiffness seen by the actuator as a function of the bending stiffness of the flexible joints is computed and shown in Figure ref:fig:delta_robot_bending_stiffness_parallel_k.
#+begin_src matlab
%% Bending Stiffness
Kfs = [1, 2, 10, 20, 50, 100, 200, 500, 1000];
Kps_Kf = zeros(size(Kfs));
for i = 1:length(Kfs)
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof', 'type_M', '3dof', 'Kf', Kfs(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
Kps_Kf(i) = (1-k*abs(evalfr(G(1,1), 1j*2*pi)))/abs(evalfr(G(1,1), 1j*2*pi));
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the bending stiffness of the flexible joints on the stiffness seen by the actuators
figure;
plot(Kfs, 1e-6*Kps_Kf)
xlabel('Bending Stiffness [Nm/rad]');
ylabel('Parallel Stiffness [N/um]');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_bending_stiffness_parallel_k.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_bending_stiffness_parallel_k
#+caption: Effect of the bending stiffness of the flexible joints on the stiffness seen by the actuators
#+RESULTS:
[[file:figs/delta_robot_bending_stiffness_parallel_k.png]]
The parallel stiffness is therefore proportional to the bending stiffness.
The "linear coefficient" depend on the geometry, and it is here equal to $3200 \frac{N/m}{Nm/\text{rad}}$.
If we want the parallel stiffness to be much smaller than the stiffness of the actuator ($k_p \ll k_a = 1.6\,N/\mu m$), the bending stiffness should be $\ll 500\,Nm/\text{rad}$.
Therefore, we should aim at $k_f < 50\,Nm/\text{rad}$.
This should be validated with the final geometry.
** Bending Stiffness
Then, the dynamics is identified for a bending Stiffness of $50\,Nm/\text{rad}$ and compared with a Delta robot with no bending stiffness in Figure ref:fig:delta_robot_bending_stiffness_dynamics.
It can be seen that the DC gain is a bit lower when the bending stiffness is considered and the resonance frequency is increased.
This simply means that the system stiffness is increased.
It is not critical from a dynamical point of view, it just decreases the achievable stroke as explained in the previous section.
#+begin_src matlab
%% Delta Robot with flexible joints having no bending compliance
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof', 'type_M', '3dof');
% Run the linearization
G_no_kf = linearize(mdl, io);
G_no_kf.InputName = {'F1', 'F2', 'F3'};
G_no_kf.OutputName = {'D1', 'D2', 'D3'};
%% Delta Robot with flexible joints having some bending compliance
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof', 'type_M', '3dof', 'Kf', 50);
% Run the linearization
G_kf = linearize(mdl, io);
G_kf.InputName = {'F1', 'F2', 'F3'};
G_kf.OutputName = {'D1', 'D2', 'D3'};
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the bending stiffness on the dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_no_kf(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
'DisplayName', '$D_i/F_i$, $K_f = 0$ Nm/rad');
plot(freqs, abs(squeeze(freqresp(G_kf(1, 1), freqs, 'Hz'))), 'color', colors(2,:), ...
'DisplayName', '$D_i/F_i$, $K_f = 50$ Nm/rad');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e3]);
ylim([1e-8, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_bending_stiffness_dynamics.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_bending_stiffness_dynamics
#+caption: Effect of the bending stiffness on the dynamics
#+RESULTS:
[[file:figs/delta_robot_bending_stiffness_dynamics.png]]
** Axial Stiffness
Now, the effect of the axial stiffness on the dynamics is studied (Figure ref:fig:delta_robot_axial_stiffness_dynamics).
Additional modes can be observed on the plant dynamics, which could limit the achievable bandwidth.
Therefore the axial stiffness should be maximized.
Having the axial stiffness 100 times stiffer than the actuator stiffness seems reasonable.
Therefore, we should aim at $k_a > 100\,N/\mu m$.
#+begin_src matlab
%% Bending Stiffness
Kas = [5e8, 2e8, 1e8, 5e7, 2e7, 1e7];
Gka = {zeros(size(Kas))};
for i = 1:length(Kas)
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '2dof_a', 'type_M', '2dof_a', 'Ka', Kas(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
Gka(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the joint's axial stiffness on the plant dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(Kas)
plot(freqs, abs(squeeze(freqresp(Gka{i}(1, 1), freqs, 'Hz'))), ...
'DisplayName', sprintf('$D_i/F_i$, $k_a = %.0f N/\\mu m$', 1e-6*Kas(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
xlim([1, 1e4]);
ylim([1e-10, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_axial_stiffness_dynamics.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:delta_robot_axial_stiffness_dynamics
#+caption: Effect of the joint's axial stiffness on the plant dynamics
#+RESULTS:
[[file:figs/delta_robot_axial_stiffness_dynamics.png]]
** Torsional Stiffness
Now the compliance in torsion of the flexible joints is considered.
If we look at the compliance of the delta robot in rotation as a function of the torsional stiffness of the flexible joints (Figure ref:fig:delta_robot_kt_compliance), we see almost no effect: the system is not made more stiff by increasing the torsional stiffness of the joints.
#+begin_src matlab
%% Effect of torsional stiffness on the system compliance
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/6dof_metrology'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Torsional Stiffness
Kts = [1, 10, 100, 1000, 10000];
Gkt = {zeros(size(Kts))};
for i = 1:length(Kts)
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '3dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', Kts(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
Gkt(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the joint's torsional stiffness on the Delta Robot compliance
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(Kts)
plot(freqs, abs(squeeze(freqresp(Gkt{i}(4, 4), freqs, 'Hz'))), ...
'DisplayName', sprintf('$R_x/M_x$, $k_t = %.0f$ Nm/rad', Kts(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
% ylim([1e-10, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_kt_compliance.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_kt_compliance
#+caption: Effect of the joint's torsional stiffness on the Delta Robot compliance
#+RESULTS:
[[file:figs/delta_robot_kt_compliance.png]]
If we have a look at the effect of the torsional stiffness on the plant dynamics (Figure ref:fig:delta_robot_kt_dynamics), we see almost no effect, except when super high values are reached ($10^6\,Nm/\text{rad}$), which are unrealistic.
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Torsional Stiffness
Kts = [1, 10, 100, 1000, 10000];
Gkt = {zeros(size(Kts))};
for i = 1:length(Kts)
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '3dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', Kts(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
Gkt(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the joint's torsional stiffness on the Delta Robot plant dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(Kts)
plot(freqs, abs(squeeze(freqresp(Gkt{i}(1, 1), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$D_1/F_1$, $k_t = %.0f$ Nm/rad', Kts(i)));
end
plot(freqs, abs(squeeze(freqresp(Gkt{i}(1, 2), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'DisplayName', 'coupling');
for i = 2:length(Kts)
plot(freqs, abs(squeeze(freqresp(Gkt{i}(1, 2), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', '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 replace
exportFig('figs/delta_robot_kt_dynamics.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_kt_dynamics
#+caption: Effect of the joint's torsional stiffness on the Delta Robot plant dynamics
#+RESULTS:
[[file:figs/delta_robot_kt_dynamics.png]]
Therefore, the torsional stiffness is not a super important metric for the design of the delta robot.
** Shear Stiffness
As shown in Figure ref:fig:delta_robot_shear_stiffness_compliance, the shear stiffness of the flexible joints has some effect on the compliance in translation and almost no effect on the compliance in rotation.
This is quite logical, and so the shear stiffness should be maximized.
A value of $100\,N/\mu m$ seems reasonable.
#+begin_src matlab
%% Effect of torsional stiffness on the system compliance
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/6dof_metrology'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Torsional Stiffness
Kss = [1e6, 1e7, 1e8, 1e9];
Gks = {zeros(size(Kss))};
for i = 1:length(Kss)
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '6dof', 'type_M', '6dof', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion, 'Ks', Kss(i));
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
Gks(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the shear stiffness of the flexible joints on the Delta Robot compliance
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(Kss)
plot(freqs, abs(squeeze(freqresp(Gks{i}(1, 1), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$D_x/F_x$, $k_s = %.0f$ N/$\\mu$m', 1e-6*Kss(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-10, 1e-4])
ax2 = nexttile();
hold on;
for i = 1:length(Kss)
plot(freqs, abs(squeeze(freqresp(Gks{i}(4, 4), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$R_x/M_x$, $k_s = %.0f N/\\mu m$', 1e-6*Kss(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-8, 1e-2])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_shear_stiffness_compliance.pdf', 'width', 'full', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_shear_stiffness_compliance
#+caption: Effect of the shear stiffness of the flexible joints on the Delta Robot compliance
#+RESULTS:
[[file:figs/delta_robot_shear_stiffness_compliance.png]]
** Effect of cube's size
*** Introduction :ignore:
Let's choose reasonable values for the flexible joints:
- Bending stiffness of 50Nm/rad
- Torsional stiffness of 500Nm/rad
- Axial stiffness of 100N/um
- Shear stiffness of 100N/um
And we see the effect of changing the cube's size.
*** Effect on the plant dynamics
- [ ] *Understand why such different dynamics between 3dof_a joints and 6dof joints with very high shear stiffnesses*
The effect of the cube's size on the plant dynamics is shown in Figure ref:fig:delta_robot_cube_size_plant_dynamics:
- coupling decreases with the cube's size
- one resonance frequency increases with the cube's size (resonances in rotation), which may be beneficial from a control point of view
- coupling at the main resonance varies with the cube's size, but it may also depend on the relative position between the CoM and the cube's center
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_shear = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Cube's size
cube_sizes = [10e-3, 20e-3, 50e-3, 100e-3];
G_cube_size = {zeros(size(cube_sizes))};
for i = 1:length(cube_sizes)
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', cube_sizes(i), 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
% delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '3dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion, 'Ks', joint_shear);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '6dof', 'type_M', '6dof', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion, 'Ks', joint_shear);
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
G_cube_size(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the cube's size on the plant dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(cube_sizes)
plot(freqs, abs(squeeze(freqresp(G_cube_size{i}(1, 1), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$D_1/F_1$, $d = %.0f$ mm', 1e3*cube_sizes(i)));
end
plot(freqs, abs(squeeze(freqresp(G_cube_size{1}(1, 2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
'DisplayName', 'coupling');
for i = 2:length(cube_sizes)
plot(freqs, abs(squeeze(freqresp(G_cube_size{i}(1, 2), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_cube_size{i}(1, 3), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'HandleVisibility', 'off');
end
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-12, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_cube_size_plant_dynamics.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_cube_size_plant_dynamics
#+caption: Effect of the cube's size on the plant dynamics
#+RESULTS:
[[file:figs/delta_robot_cube_size_plant_dynamics.png]]
*** Effect on the compliance
As shown in Figure ref:fig:delta_robot_cube_size_compliance_rotation, the stiffness of the delta robot in rotation increases with the cube's size.
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/6dof_metrology'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Cube's size
cube_sizes = [10e-3, 20e-3, 50e-3, 100e-3];
G_cube_size = {zeros(size(cube_sizes))};
for i = 1:length(cube_sizes)
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', cube_sizes(i), 'b', b, 'L', L);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '2dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion);
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
G_cube_size(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the cube's size on the rotational compliance of the top platform
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(cube_sizes)
plot(freqs, abs(squeeze(freqresp(G_cube_size{i}(4, 4), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$R_x/M_x$, $d = %.0f$ mm', 1e3*cube_sizes(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-6, 1e-1])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_cube_size_compliance_rotation.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_cube_size_compliance_rotation
#+caption: Effect of the cube's size on the rotational compliance of the top platform
#+RESULTS:
[[file:figs/delta_robot_cube_size_compliance_rotation.png]]
With a cube size of 50mm, the resonance frequency is already above 1kHz with seems reasonable.
** Effect of the strut length ?
*** Introduction :ignore:
Let's choose reasonable values for the flexible joints:
- Bending stiffness of 50Nm/rad
- Torsional stiffness of 500Nm/rad
- Axial stiffness of 100N/um
And we see the effect of changing the strut length.
*** Effect on the plant dynamics
As shown in Figure ref:fig:delta_robot_strut_length_plant_dynamics, having longer struts:
- decreases the main resonance frequency: this means that the stiffness in the X,Y and Z directions is decreased when the length of the strut is longer.
This is reasonable as the "lever" arm is getting larger, so the bending stiffness and compression of the flexible joints have a larger effect on the top platform compliance.
- decreases the low frequency coupling: this effect is more difficult to physically understand
But even with relatively short struts (20mm and above), the low frequency decoupling is already around two orders of magnitude, which is enough from a control point of view.
So, the struts length can be optimized to not decrease too much the stiffness of the platform while still getting good low frequency decoupling.
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Cube's size
strut_length = [10e-3, 20e-3, 50e-3, 100e-3];
G_strut_length = {zeros(size(strut_length))};
for i = 1:length(strut_length)
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', strut_length(i));
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '2dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion);
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'D1', 'D2', 'D3'};
G_strut_length(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the cube's size on the plant dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(strut_length)
plot(freqs, abs(squeeze(freqresp(G_strut_length{i}(1, 1), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$D_1/F_1$, $L = %.0f$ mm', 1e3*strut_length(i)));
end
plot(freqs, abs(squeeze(freqresp(G_strut_length{1}(1, 2), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
'DisplayName', 'coupling');
for i = 2:length(strut_length)
plot(freqs, abs(squeeze(freqresp(G_strut_length{i}(1, 2), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_strut_length{i}(1, 3), freqs, 'Hz'))), 'color', [colors(i,:), 0.2], ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-12, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_strut_length_plant_dynamics.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_strut_length_plant_dynamics
#+caption: Effect of the cube's size on the plant dynamics
#+RESULTS:
[[file:figs/delta_robot_strut_length_plant_dynamics.png]]
*** Effect on the compliance
As shown in Figure ref:fig:delta_robot_strut_length_compliance_rotation, the strut length has an effect on the system stiffness in translation (left plot) but almost not in rotation (right plot).
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fd'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/6dof_metrology'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
% Cube's size
strut_length = [10e-3, 20e-3, 50e-3, 100e-3];
G_strut_length = {zeros(size(strut_length))};
for i = 1:length(strut_length)
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', strut_length(i));
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '2dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion);
delta_robot = initializeCylindricalStruts(delta_robot);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
G_strut_length(i) = {G};
end
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the cube's size on the rotational compliance of the top platform
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 2, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
for i = 1:length(strut_length)
plot(freqs, abs(squeeze(freqresp(G_strut_length{i}(1, 1), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$D_x/F_x$, $L = %.0f$ mm', 1e3*strut_length(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-10, 1e-4])
ax2 = nexttile();
hold on;
for i = 1:length(strut_length)
plot(freqs, abs(squeeze(freqresp(G_strut_length{i}(4, 4), freqs, 'Hz'))), 'color', colors(i,:), ...
'DisplayName', sprintf('$R_x/M_x$, $L = %.0f$ mm', 1e3*strut_length(i)));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [rad/Nm]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-8, 1e-2])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_strut_length_compliance_rotation.pdf', 'width', 'full', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_strut_length_compliance_rotation
#+caption: Effect of the cube's size on the rotational compliance of the top platform
#+RESULTS:
[[file:figs/delta_robot_strut_length_compliance_rotation.png]]
** Having the Center of Mass at the cube's center
To make things easier, we take a top platform with no mass, mass-less struts, and we put a payload on top of the platform.
As shown in Figure ref:fig:delta_robot_CoM_pos_effect_plant, having the CoM of the payload at the cube's center allow to have better decoupling properties above the suspension mode of the system (i.e. above the first mode).
This could allow to have a bandwidth exceeding the frequency of the first mode.
But how sensitive this decoupling is to the exact position of the CoM still need to be studied.
#+begin_src matlab
%% Effect of torsional stiffness on the plant dynamics
joint_axial = 100e6; % [N/m]
joint_bending = 50; % [Nm/rad]
joint_torsion = 500; % [Nm/rad]
% Geometry
d = 50e-3; % Cube's edge length [m]
b = 20e-3; % Distance between cube's vertices and top joints [m]
L = 50e-3; % Strut length
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/delta_robot'], 1, 'openoutput'); io_i = io_i + 1; % Actuator Displacement [m]
delta_robot = initializeStewartPlatform();
delta_robot = generateDeltaRobot(delta_robot, 'd', d, 'b', b, 'L', L, 'm_top', 1e-3);
delta_robot = computeJointsPose(delta_robot);
delta_robot = initializeActuatorDynamics(delta_robot, 'type', '1dof', 'k', k);
delta_robot = initializeJointDynamics(delta_robot, 'type_F', '3dof_a', 'type_M', '2dof_a', 'Ka', joint_axial, 'Kf', joint_bending, 'Kt', joint_torsion);
delta_robot = initializeCylindricalStruts(delta_robot, 'M', 1e-3);
delta_robot = computeJacobian(delta_robot);
delta_robot = initializeStewartPose(delta_robot);
#+end_src
#+begin_src matlab
%% Sample at the CoM
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', 2*(L/2+b)*1/sqrt(3), 'R', 20e-3);
G_com = linearize(mdl, io);
G_com.InputName = {'F1', 'F2', 'F3'};
G_com.OutputName = {'D1', 'D2', 'D3'};
%% Sample above the CoM
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', 4*(L/2+b)*1/sqrt(3), 'R', 20e-3);
G_above = linearize(mdl, io);
G_above.InputName = {'F1', 'F2', 'F3'};
G_above.OutputName = {'D1', 'D2', 'D3'};
%% Sample below the CoM
sample = initializeSample('type', 'cylindrical', 'm', 1, 'H', (L/2+b)*1/sqrt(3), 'R', 20e-3);
G_below = linearize(mdl, io);
G_below.InputName = {'F1', 'F2', 'F3'};
G_below.OutputName = {'D1', 'D2', 'D3'};
#+end_src
#+begin_src matlab :exports none :results none
%% Effect of the payload's Center of Mass position with respect to the cube's size on the plant dynamics
freqs = logspace(0, 4, 1000);
figure;
tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
ax1 = nexttile();
hold on;
plot(freqs, abs(squeeze(freqresp(G_com(1, 1), freqs, 'Hz'))), 'color', colors(1,:), ...
'DisplayName', 'CoM');
plot(freqs, abs(squeeze(freqresp(G_above(1, 1), freqs, 'Hz'))), 'color', colors(2,:), ...
'DisplayName', 'Above');
plot(freqs, abs(squeeze(freqresp(G_below(1, 1), freqs, 'Hz'))), 'color', colors(3,:), ...
'DisplayName', 'Below');
plot(freqs, abs(squeeze(freqresp(G_com(1, 2), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
'DisplayName', 'coupling');
plot(freqs, abs(squeeze(freqresp(G_com(1, 3), freqs, 'Hz'))), 'color', [colors(1,:), 0.5], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_above(1, 2), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_above(1, 3), freqs, 'Hz'))), 'color', [colors(2,:), 0.5], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_below(1, 2), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_below(1, 3), freqs, 'Hz'))), 'color', [colors(3,:), 0.5], ...
'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
leg.ItemTokenSize(1) = 15;
% xlim([1, 1e4]);
ylim([1e-12, 1e-4])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/delta_robot_CoM_pos_effect_plant.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:delta_robot_CoM_pos_effect_plant
#+caption: Effect of the payload's Center of Mass position with respect to the cube's size on the plant dynamics
#+RESULTS:
[[file:figs/delta_robot_CoM_pos_effect_plant.png]]
** Conclusion
* Conclusion
* Bibliography :ignore:
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
* Matlab Functions :noexport:
** =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 -
% - struts -
% - actuators -
% - geometry -
% - properties -
stewart = struct();
stewart.platform_F = struct();
stewart.platform_M = struct();
stewart.joints = struct();
stewart.struts = 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_src
** =generateDeltaRobot=: Generate a Delta-Robot Configuration
#+begin_src matlab :tangle matlab/src/generateDeltaRobot.m :comments none :mkdirp yes :eval no
function [stewart] = generateDeltaRobot(stewart, args)
% generateDeltaRobot - Generate a Delta Robot Configuration
%
% Syntax: [stewart] = generateDeltaRobot(args)
%
% Inputs:
% - args - Can have the following fields:
% - d [1x1] - Size of the cube [m]
% - b [1x1] - Distance between top joints bi and cube's vertices [m]
% - L [1x1] - Length of the struts (i.e. distances between ai and bi) [m]
arguments
stewart
args.d (1,1) double {mustBeNumeric, mustBePositive} = 70e-3
args.b (1,1) double {mustBeNumeric, mustBeNonnegative} = 50e-3
args.L (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.m_top (1,1) double {mustBeNumeric, mustBePositive} = 0.3 % Mass of the top platform
end
% We define the useful vertices 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]);
% Coordinates of the cube's vertices
Cc = R'*[[args.d/2;-args.d/2;-args.d/2],[args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;-args.d/2],[-args.d/2;args.d/2;args.d/2],[-args.d/2;-args.d/2;args.d/2],[args.d/2;-args.d/2;args.d/2]];
% Coordinates of the cube's vertices corresponding to the six struts, expresssed with respect to the cube's center
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(:,6), Cc(:,4), Cc(:,2), Cc(:,6), Cc(:,4), Cc(:,2)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
% We can compute the vector of each leg si
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
% We now compute the position of the joints ai and bi with respect to the cube's center
Ca = CCf - (args.b + args.L).*CSi;
Cb = CCf - args.b.*CSi;
%% Computes relative position of frames
FO_M = [0; 0; args.L*CSi(3,1)]; % Position of {M} with respect to {F} [m]
MO_B = [0; 0; (args.b+args.d/2)*CSi(3,1)]; % Position of {B} with respect to {M} [m]
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
stewart.geometry.d = args.d; % Size of the cube
stewart.geometry.H = args.L*CSi(3,1); % Height between {M} and {F} frames
stewart.geometry.FO_M = FO_M;
stewart.platform_M.MO_B = MO_B;
stewart.platform_F.FO_A = FO_A;
%% Now ai and bi can be expressed with respect to the {F} and {M} frames
Fa = Ca + FO_A;
Mb = Cb + MO_B;
%%
stewart.platform_F.Ca = Ca; % Position of bottom joints ai with respect to {F}
stewart.platform_F.Fa = Fa; % Position of bottom joints ai with respect to {F}
stewart.platform_M.Mb = Mb; % Position of top joints bi with respect to {M}
stewart.platform_M.Cb = Cb; % Position of top joints bi with respect to {M}
stewart.platform_M.m = args.m_top;
#+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, 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}
% - 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.platform_F.FRa = FRa;
stewart.platform_M.MRb = MRb;
#+end_src
** =initializeActuatorDynamics=: Add Stiffness and Damping properties of each strut
#+begin_src matlab :tangle matlab/src/initializeActuatorDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeActuatorDynamics(stewart, args)
% initializeActuatorDynamics - Add Stiffness and Damping properties of each strut
%
% Syntax: [stewart] = initializeActuatorDynamics(args)
%
% Inputs:
% - args - Structure with the following fields:
% - K [6x1] - Stiffness of each strut [N/m]
% - C [6x1] - Damping of each strut [N/(m/s)]
%
% Outputs:
% - stewart - updated Stewart structure with the added fields:
% - actuators.type = 1
% - actuators.K [6x1] - Stiffness of each strut [N/m]
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
arguments
stewart
args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
args.F_gain (1,1) double {mustBeNumeric} = 1
args.me (1,1) double {mustBeNumeric} = 0.01
args.ma (1,1) double {mustBeNumeric} = 0.01
end
if strcmp(args.type, '1dof')
stewart.actuators.type = 1;
elseif strcmp(args.type, '2dof')
stewart.actuators.type = 2;
elseif strcmp(args.type, 'flexible')
stewart.actuators.type = 3;
end
stewart.actuators.k = args.k;
stewart.actuators.c = args.c;
% Parallel stiffness
stewart.actuators.kp = args.kp;
stewart.actuators.cp = args.cp;
stewart.actuators.ka = args.ka;
stewart.actuators.ca = args.ca;
stewart.actuators.ke = args.ke;
stewart.actuators.ce = args.ce;
stewart.actuators.F_gain = args.F_gain;
stewart.actuators.ma = args.ma;
stewart.actuators.me = args.me;
end
#+end_src
** =initializeJointDynamics=: Add Stiffness and Damping properties for joints
#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no
function [stewart] = initializeJointDynamics(stewart, args)
arguments
stewart
args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '2dof_a', '3dof_a', '6dof'})} = '2dof'
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '2dof_a', '3dof_a', '6dof'})} = '3dof'
args.Kf (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 % Bending stiffness
args.Cf (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Kt (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 % Torsional stiffness
args.Ct (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 % Axial stiffness
args.Ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
args.Ks (1,1) double {mustBeNumeric, mustBeNonnegative} = 0 % Shear stiffness
args.Cs (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
end
switch args.type_F
case '2dof'
stewart.joints.type_F = 1;
case '3dof'
stewart.joints.type_F = 2;
case '2dof_a'
stewart.joints.type_F = 3;
case '3dof_a'
stewart.joints.type_F = 4;
case '6dof'
stewart.joints.type_F = 5;
otherwise
error("joints are not correctly defined")
end
switch args.type_M
case '2dof'
stewart.joints.type_M = 1;
case '3dof'
stewart.joints.type_M = 2;
case '2dof_a'
stewart.joints.type_M = 3;
case '3dof_a'
stewart.joints.type_M = 4;
case '6dof'
stewart.joints.type_M = 5;
otherwise
error("joints are not correctly defined")
end
%% Axial Stiffness
stewart.joints.Ka = args.Ka;
stewart.joints.Ca = args.Ca;
%% Bending Stiffness
stewart.joints.Kf = args.Kf;
stewart.joints.Cf = args.Cf;
%% Torsional Stiffness
stewart.joints.Kt = args.Kt;
stewart.joints.Ct = args.Ct;
%% Shear Stiffness
stewart.joints.Ks = args.Ks;
stewart.joints.Cs = args.Cs;
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.M (1,1) double {mustBeNumeric, mustBePositive} = 0.1 % Mass [kg]
args.R (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius [m]
end
stewart.struts.M = args.M;
stewart.struts.R = args.R;
stewart.struts.H = stewart.geometry.l(1);
end
#+end_src
** =plotCube=: Plot the Cube
#+begin_src matlab :tangle matlab/src/plotCube.m :comments none :mkdirp yes :eval no
function [] = plotCube(stewart, args)
arguments
stewart
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Height of the cube
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
#+end_src
** =plotCylindricalPayload=: Plot a cylindrical Payload
#+begin_src matlab :tangle matlab/src/plotCylindricalPayload.m :comments none :mkdirp yes :eval no
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)
#+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:
% - 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;
#+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_src
** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
#+begin_src matlab :tangle matlab/src/forwardKinematicsApprox.m :comments none :mkdirp yes :eval no
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)];
#+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_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, 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;
#+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_src
** =displayArchitecture=: 3D plot of the Stewart platform architecture
#+begin_src matlab :tangle matlab/src/displayArchitecture.m :comments none :mkdirp yes :eval no
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
plot3([Fa(1,:), Fa(1,1)], ...
[Fa(2,:), Fa(2,1)], ...
[Fa(3,:), Fa(3,1)], '-', '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.
Fb = FTm*[Mb;ones(1,6)];
if args.platforms
plot3([Fb(1,:), Fb(1,1)], ...
[Fb(2,:), Fb(2,1)], ...
[Fb(3,:), Fb(3,1)], '-', 'Color', args.M_color);
end
% Plot the position and labels of the rotation joints fixed to the mobile platform.
if args.joints
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
#+end_src
** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
#+begin_src matlab :tangle matlab/src/describeStewartPlatform.m :comments none :mkdirp yes :eval no
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
#+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_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
#+end_src
** =initializeController=: Initialize Controller
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
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
#+end_src
* Helping Functions :noexport:
** Initialize Path
#+NAME: m-init-path
#+BEGIN_SRC matlab
%% Path for functions, data and scripts
addpath('./matlab/src/'); % Path for functions
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
addpath('./matlab/'); % Path for scripts
#+END_SRC
#+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 = 'delta_robot_model';
open(mdl)
#+end_src
** Initialize other elements
#+NAME: m-init-other
#+BEGIN_SRC matlab
%% Colors for the figures
colors = colororder;
#+END_SRC
Reference in New Issue View Git Blame Copy Permalink