#+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: #+HTML_HEAD: #+BIND: org-latex-image-default-option "scale=1" #+BIND: org-latex-image-default-width "" #+LaTeX_CLASS: scrreprt #+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc] #+LATEX_HEADER: \input{preamble.tex} #+LATEX_HEADER_EXTRA: \input{preamble_extra.tex} #+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: <> ** 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) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** Studied Geometry The Delta Robot geometry is defined as shown in Figure [[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 [[fig:delta_robot_architecture]] and [[fig:delta_robot_architecture_top]]). #+begin_src matlab :exports code %% 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] #+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); 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 There are three actuators in the following directions $\hat{s}_1$, $\hat{s}_2$ and $\hat{s}_3$; \begin{equation}\label{eq:delta_robot_unit_vectors} \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} The Jacobian matrix is defined as shown in eqref:eq:delta_robot_jacobian. \begin{equation}\label{eq:delta_robot_jacobian} \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 %% Jacobian matrix s1 = delta_robot.geometry.As(:,1); s2 = delta_robot.geometry.As(:,3); s3 = delta_robot.geometry.As(:,5); J = [s1' ; s2' ; s3'] #+end_src It links the small actuator displacement to the top platform displacement eqref:eq:delta_robot_inverse_kinematics. \begin{equation}\label{eq:delta_robot_inverse_kinematics} d\mathcal{L} = J d\mathcal{L} \end{equation} \begin{equation}\label{eq:delta_robot_forward_kinematics} 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 %% Estimation of the mobility 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 and worst case 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 and worst case #+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'}; dcgain(G_kt) #+end_src 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'}; dcgain(G_ka) #+end_src 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'}; dcgain(G_ka_kt) #+end_src 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 <> ** Introduction :ignore: First, in Section [[ssec:delta_robot_flexible_joints_geometry]], 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. The goal is to extract specifications for the flexible joints of the six struts, in terms of: - bending stiffness (Section [[ssec:delta_robot_flexible_joints_bending]]) - axial stiffness (Section [[ssec:delta_robot_flexible_joints_axial]]) - torsional stiffness (Section [[ssec:delta_robot_flexible_joints_torsion]]) - shear stiffness (Section [[ssec:delta_robot_flexible_joints_shear]]) ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** 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 []. #+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 [[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]] ** Bending Stiffness <> *** Stiffness seen by the actuator, and decrease of the achievable stroke 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 [[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. 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 [[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]] *** Effect on the coupling Here, reasonable values for the flexible joints (modelled as a 6DoF joint) stiffness are taken: - Torsional stiffness of 500Nm/rad - Axial stiffness of 100N/um - Shear stiffness of 100N/um And the bending stiffness is varied from low to high values. The obtained dynamics is shown in Figure [[fig:delta_robot_bending_stiffness_couplign]]. It can be seen that the low frequency coupling increases when the bending stiffness increases. Therefore, the bending stiffness of the flexible joints should be minimized (10Nm/rad could be a reasonable objective). #+begin_src matlab %% Effect of bending stiffness on the plant dynamics 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] 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 = initializeCylindricalStruts(delta_robot); delta_robot = computeJacobian(delta_robot); delta_robot = initializeStewartPose(delta_robot); 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] Kfs = [1, 10, 100, 1000]; Gkf = {zeros(size(Kfs))}; for i = 1:length(Kfs) delta_robot = initializeJointDynamics(delta_robot, 'type_F', '6dof', 'type_M', '6dof', 'Ka', joint_axial, 'Kf', Kfs(i), 'Kt', joint_torsion, 'Ks', joint_shear); %% Run the linearization G = linearize(mdl, io); G.InputName = {'F1', 'F2', 'F3'}; G.OutputName = {'D1', 'D2', 'D3'}; Gkf(i) = {G}; end #+end_src #+begin_src matlab :exports none :results none %% Effect of the bending stiffness of the flexible joints on the coupling freqs = logspace(0, 4, 1000); figure; tiledlayout(1, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); ax1 = nexttile(); hold on; for i = 1:length(Kfs) plot(freqs, abs(squeeze(freqresp(Gkf{i}(1, 1), freqs, 'Hz'))), ... 'DisplayName', sprintf('$D_i/F_i$, $k_f = %.0f Nm/rad$', Kfs(i))); end plot(freqs, abs(squeeze(freqresp(Gkf{1}(1, 2), freqs, 'Hz'))), 'color', [colors(1, :), 0.5], ... 'DisplayName', 'coupling'); plot(freqs, abs(squeeze(freqresp(Gkf{1}(1, 3), freqs, 'Hz'))), 'color', [colors(1, :), 0.5], ... 'HandleVisibility', 'off'); for i = 2:length(Kfs) plot(freqs, abs(squeeze(freqresp(Gkf{i}(1, 2), freqs, 'Hz'))), 'color', [colors(i, :), 0.5], ... 'HandleVisibility', 'off'); plot(freqs, abs(squeeze(freqresp(Gkf{i}(1, 3), freqs, 'Hz'))), 'color', [colors(i, :), 0.5], ... 'HandleVisibility', 'off'); 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-12, 1e-4]) #+end_src #+begin_src matlab :tangle no :exports results :results file replace exportFig('figs/delta_robot_bending_stiffness_couplign.pdf', 'width', 'wide', 'height', 'normal'); #+end_src #+name: fig:delta_robot_bending_stiffness_couplign #+caption: Effect of the bending stiffness of the flexible joints on the coupling #+RESULTS: [[file:figs/delta_robot_bending_stiffness_couplign.png]] ** Axial Stiffness <> Now, the effect of the axial stiffness on the dynamics is studied (Figure [[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 [[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 [[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 [[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]] ** Conclusion #+name: tab:delta_robot_flexible_joints_recommendations #+caption: Recommendations for the flexible joints | Joint's Stiffness | Effect | Recommendation | |-------------------+--------------------------------------------------+-------------------------------------------------------------------------------------------| | Bending | Can reduce the stroke, and increase the coupling | Below 50 to 10 Nm/rad | | Axial | Add modes that can limit the feedback bandwidth | As high as possible, at least 100 Nm/um | | Torsion | Minor effect | No recommendation | | Shear | Can limit the stiffness of the system | As high as possible (less important than the axial stiffness), above 100 N/um if possible | * Effect of the Geometry <> ** 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) <> #+end_src #+begin_src matlab :exports none :results silent :noweb yes <> #+end_src #+begin_src matlab :tangle no :noweb yes <> #+end_src #+begin_src matlab :eval no :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src #+begin_src matlab :noweb yes <> #+end_src ** 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 [[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 [[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 compliance As shown in Figure [[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). Indeed, the stiffness in rotation is a combination of: - The stiffness of the actuator - The shear and axial stiffness of the flexible joints - The bending and torsional stiffness of the flexible joints, combine with the strut length #+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]] *** Effect on the plant dynamics As shown in Figure [[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 Probably: when pushing with one actuator, it induces some rotation of the struts corresponding to the other two actuators. This rotation is proportional to the strut length. Then, this rotation, combined with the limited compliance in bending of the flexible joints induces some force applied on the other actuators, hence the coupling. This is similar to what was observed when varying the bending stiffness of the flexible joints: the coupling was increased with an increased of the bending stiffness (See Figure [[fig:delta_robot_bending_stiffness_couplign]]) *So we should also observed a decrease of the coupling when decreasing the bending stiffness of the actuators* 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 Strut length on the plant dynamics #+RESULTS: [[file:figs/delta_robot_strut_length_plant_dynamics.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 [[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