Table of Contents
-
-
- 1. Jacobian Analysis +
- 1. Jacobian Analysis -
- 2. Stiffness Analysis +
- 2. Stiffness Analysis -
- 3. Forward and Inverse Kinematics +
- 3. Forward and Inverse Kinematics
-
-
- 3.1. Inverse Kinematics -
- 3.2. Forward Kinematics -
- 3.3. Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix -
- 3.4. Estimation of the range validity of the approximate inverse kinematics +
- 3.1. Inverse Kinematics +
- 3.2. Forward Kinematics +
- 3.3. Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix +
- 3.4. Estimation of the range validity of the approximate inverse kinematics
- - 4. Estimated required actuator stroke from specified platform mobility +
- 4. Estimated required actuator stroke from specified platform mobility
-
-
- 4.1. Stewart architecture definition -
- 4.2. Wanted translations and rotations -
- 4.3. Needed stroke for “pure” rotations or translations -
- 4.4. Needed stroke for “combined” rotations or translations +
- 4.1. Stewart architecture definition +
- 4.2. Wanted translations and rotations +
- 4.3. Needed stroke for “pure” rotations or translations +
- 4.4. Needed stroke for “combined” rotations or translations
- - 5. Estimated platform mobility from specified actuator stroke +
- 5. Estimated platform mobility from specified actuator stroke -
- 6. Functions +
- 6. Functions
-
-
- 6.1.
computeJacobian: Compute the Jacobian Matrix + - 6.1.
computeJacobian: Compute the Jacobian Matrix
- - 6.2.
inverseKinematics: Compute Inverse Kinematics + - 6.2.
inverseKinematics: Compute Inverse Kinematics
- - 6.3.
forwardKinematicsApprox: Compute the Approximate Forward Kinematics + - 6.3.
forwardKinematicsApprox: Compute the Approximate Forward Kinematics
-
-
- Section 1: The Jacobian matrix is derived from the geometry of the Stewart platform. Then it is shown that the Jacobian can link velocities and forces present in the system, and thus this matrix can be very useful for both analysis and control of the Stewart platform. -
- Section 2: The stiffness and compliance matrices are derived from the Jacobian matrix and the stiffness of each strut. -
- Section 3: The Forward and Inverse kinematic problems are presented. -
- Section 4: The Inverse kinematic solution is used to estimate required actuator stroke from the wanted mobility of the Stewart platform. +
- Section 1: The Jacobian matrix is derived from the geometry of the Stewart platform. Then it is shown that the Jacobian can link velocities and forces present in the system, and thus this matrix can be very useful for both analysis and control of the Stewart platform. +
- Section 2: The stiffness and compliance matrices are derived from the Jacobian matrix and the stiffness of each strut. +
- Section 3: The Forward and Inverse kinematic problems are presented. +
- Section 4: The Inverse kinematic solution is used to estimate required actuator stroke from the wanted mobility of the Stewart platform.
-1 Jacobian Analysis
++1 Jacobian Analysis
-From taghirad13_paral: @@ -374,8 +378,8 @@ The Jacobian matrix not only reveals the relation between the joint variable
-1.1 Jacobian Computation
++1.1 Jacobian Computation
-If we note: @@ -400,7 +404,7 @@ Then, we can compute the Jacobian with the following equation (the superscript \ \end{equation*}
-The Jacobian matrix \(\bm{J}\) can be computed using the
computeJacobianfunction (accessible here). +The Jacobian matrix \(\bm{J}\) can be computed using thecomputeJacobianfunction (accessible here). For instance:@@ -418,8 +422,8 @@ This will add three new matrix to thestewartstructure:-1.2 Jacobian - Velocity loop closure
++-1.2 Jacobian - Velocity loop closure
The Jacobian matrix links the input joint rate \(\dot{\bm{\mathcal{L}}} = [ \dot{l}_1, \dot{l}_2, \dot{l}_3, \dot{l}_4, \dot{l}_5, \dot{l}_6 ]^T\) of each strut to the output twist vector of the mobile platform is denoted by \(\dot{\bm{X}} = [^A\bm{v}_p, {}^A\bm{\omega}]^T\): @@ -442,13 +446,13 @@ If the Jacobian matrix is inversible, we can also compute \(\dot{\bm{\mathcal{X}
The Jacobian matrix can also be used to approximate forward and inverse kinematics for small displacements. -This is explained in section 3.3. +This is explained in section 3.3.
-1.3 Jacobian - Static Force Transformation
++-1.3 Jacobian - Static Force Transformation
If we note: @@ -475,19 +479,19 @@ If the Jacobian matrix is inversible, we also have the following relation:
-2 Stiffness Analysis
++2 Stiffness Analysis
-Here, we focus on the deflections of the manipulator moving platform that are the result of the external applied wrench to the mobile platform. The amount of these deflections are a function of the applied wrench as well as the manipulator structural stiffness.
--2.1 Computation of the Stiffness and Compliance Matrix
++2.1 Computation of the Stiffness and Compliance Matrix
As explain in this document, each Actuator is modeled by 3 elements in parallel: @@ -538,24 +542,24 @@ The compliance matrix of a manipulator shows the mapping of the moving platform \end{equation*}
-The stiffness and compliance matrices are computed using the
computeJacobianfunction (accessible here). +The stiffness and compliance matrices are computed using thecomputeJacobianfunction (accessible here).-3 Forward and Inverse Kinematics
++3 Forward and Inverse Kinematics
--3.1 Inverse Kinematics
++-3.1 Inverse Kinematics
-3.2 Forward Kinematics
++-3.2 Forward Kinematics
-3.3 Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix
++-3.3 Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix
@@ -639,15 +643,18 @@ As the inverse kinematic can be easily solved exactly this is not much useful, h
-The function
forwardKinematicsApprox(described here) can be used to solve the forward kinematic problem using the Jacobian matrix. +The functionforwardKinematicsApprox(described here) can be used to solve the forward kinematic problem using the Jacobian matrix.-3.4 Estimation of the range validity of the approximate inverse kinematics
++3.4 Estimation of the range validity of the approximate inverse kinematics
+-As we know how to exactly solve the Inverse kinematic problem, we can compare the exact solution with the approximate solution using the Jacobian matrix. For small displacements, the approximate solution is expected to work well. We would like here to determine up to what displacement this approximation can be considered as correct. @@ -659,8 +666,8 @@ This will also gives us the range for which the approximate forward kinematic is
-3.4.1 Stewart architecture definition
++-3.4.1 Stewart architecture definition
We first define some general Stewart architecture. @@ -680,8 +687,8 @@ stewart = computeJacobian(stewart);
-3.4.2 Comparison for “pure” translations
++3.4.2 Comparison for “pure” translations
Let’s first compare the perfect and approximate solution of the inverse for pure \(x\) translations. @@ -689,8 +696,8 @@ Let’s first compare the perfect and approximate solution of the inverse fo
We compute the approximate and exact required strut stroke to have the wanted mobile platform \(x\) displacement. -The estimate required strut stroke for both the approximate and exact solutions are shown in Figure 1. -The relative strut length displacement is shown in Figure 2. +The estimate required strut stroke for both the approximate and exact solutions are shown in Figure 1. +The relative strut length displacement is shown in Figure 2.
-Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m] @@ -707,14 +714,14 @@ Ls_exact = zeros(6, length(Xrs));
+-
Figure 1: Comparison of the Approximate solution and True solution for the Inverse kinematic problem (png, pdf)
+--3.4.3 Conclusion
++-3.4.3 Conclusion
For small wanted displacements (up to \(\approx 1\%\) of the size of the Hexapod), the approximate inverse kinematic solution using the Jacobian matrix is quite correct. @@ -733,11 +740,11 @@ For small wanted displacements (up to \(\approx 1\%\) of the size of the Hexapod
-4 Estimated required actuator stroke from specified platform mobility
++4 Estimated required actuator stroke from specified platform mobility
-Let’s say one want to design a Stewart platform with some specified mobility (position and orientation). @@ -745,8 +752,8 @@ One may want to determine the required actuator stroke required to obtain the sp This is what is analyzed in this section.
-4.1 Stewart architecture definition
++-4.1 Stewart architecture definition
Let’s first define the Stewart platform architecture that we want to study. @@ -766,8 +773,8 @@ stewart = computeJacobian(stewart);
-4.2 Wanted translations and rotations
++-4.2 Wanted translations and rotations
Let’s now define the wanted extreme translations and rotations. @@ -784,8 +791,8 @@ Rz_max = 0; % Rotation [rad]
-4.3 Needed stroke for “pure” rotations or translations
++-4.3 Needed stroke for “pure” rotations or translations
As a first estimation, we estimate the needed actuator stroke for “pure” rotations and translation. @@ -816,8 +823,8 @@ This is surely a low estimation of the required stroke.
-4.4 Needed stroke for “combined” rotations or translations
++-4.4 Needed stroke for “combined” rotations or translations
We know would like to have a more precise estimation. @@ -1137,23 +1144,23 @@ This is probably a much realistic estimation of the required actuator stroke.
-5 Estimated platform mobility from specified actuator stroke
++5 Estimated platform mobility from specified actuator stroke
-Here, from some value of the actuator stroke, we would like to estimate the mobility of the Stewart platform.
-As explained in section 3, the forward kinematic problem of the Stewart platform is quite difficult to solve. +As explained in section 3, the forward kinematic problem of the Stewart platform is quite difficult to solve. However, for small displacements, we can use the Jacobian as an approximate solution.
-5.1 Stewart architecture definition
++5.1 Stewart architecture definition
-Let’s first define the Stewart platform architecture that we want to study. @@ -1182,8 +1189,8 @@ L_max = 50e-6; % [m]
-5.2 Pure translations
++5.2 Pure translations
Let’s first estimate the mobility in translation when the orientation of the Stewart platform stays the same. @@ -1255,7 +1262,7 @@ We can also approximate the mobility by a sphere with a radius equal to the mini -
+--6 Functions
++6 Functions
--6.1
+computeJacobian: Compute the Jacobian Matrix+6.1
computeJacobian: Compute the Jacobian Matrix-@@ -1283,9 +1290,9 @@ This Matlab function is accessible here.
-Function description
-++-Function description
+function [stewart] = computeJacobian(stewart) % computeJacobian - @@ -1294,55 +1301,87 @@ This Matlab function is accessible here. % % Inputs: % - stewart - With at least the following fields: -% - As [3x6] - The 6 unit vectors for each strut expressed in {A} -% - Ab [3x6] - The 6 position of the joints bi expressed in {A} +% - 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: -% - J [6x6] - The Jacobian Matrix -% - K [6x6] - The Stiffness Matrix -% - C [6x6] - The Compliance Matrix +% - kinematics.J [6x6] - The Jacobian Matrix +% - kinematics.K [6x6] - The Stiffness Matrix +% - kinematics.C [6x6] - The Compliance Matrix
-Compute Jacobian Matrix
-++-Check the
+stewartstructure elements-stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)']; +
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;
-Compute Stiffness Matrix
-+ ++-Compute Jacobian Matrix
+-stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J; +
J = [As' , cross(Ab, As)'];
-Compute Compliance Matrix
-+-++ +Compute Stiffness Matrix
++-+stewart.C = inv(stewart.K); +
K = J'*diag(Ki)*J; +
+++ +Compute Compliance Matrix
+++++C = inv(K); +
++Populate the
+stewartstructure++stewart.kinematics.J = J; +stewart.kinematics.K = K; +stewart.kinematics.C = C;
-6.2
+ +inverseKinematics: Compute Inverse Kinematics+6.2
inverseKinematics: Compute Inverse Kinematics-@@ -1350,48 +1389,9 @@ This Matlab function is accessible here.
-- -Function description
-----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 -% - 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: -% - 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} -
--- -Optional Parameters
-----arguments - stewart - args.AP (3,1) double {mustBeNumeric} = zeros(3,1) - args.ARB (3,3) double {mustBeNumeric} = eye(3) -end ---Theory
-++-Theory
+For inverse kinematic analysis, it is assumed that the position \({}^A\bm{P}\) and orientation of the moving platform \({}^A\bm{R}_B\) are given and the problem is to obtain the joint variables, namely, \(\bm{L} = [l_1, l_2, \dots, l_6]^T\).
@@ -1425,27 +1425,85 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then the-Compute
-+-++ +Function description
++-+Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa)); +
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} +
+++ +Optional Parameters
+++++arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) +end ++++ + +Check the
+stewartstructure elements++++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; +
++Compute
+++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-stewart.l; +
dLi = Li-l;-6.3
+forwardKinematicsApprox: Compute the Approximate Forward Kinematics+6.3
forwardKinematicsApprox: Compute the Approximate Forward Kinematics-@@ -1453,9 +1511,9 @@ This Matlab function is accessible here<
-Function description
-++Function description
+-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 @@ -1465,7 +1523,7 @@ This Matlab function is accessible here< % % Inputs: % - stewart - A structure with the following fields -% - J [6x6] - The Jacobian Matrix +% - kinematics.J [6x6] - The Jacobian Matrix % - args - Can have the following fields: % - dL [6x1] - Displacement of each strut [m] % @@ -1477,9 +1535,9 @@ This Matlab function is accessible here<
-Optional Parameters
-+-+Optional Parameters
+-arguments stewart @@ -1490,16 +1548,27 @@ This Matlab function is accessible here<-Computation
-++++ +Check the
+stewartstructure elements++++assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') +J = stewart.kinematics.J; +
++Computation
+From a small displacement of each strut \(d\bm{\mathcal{L}}\), we can compute the position and orientation of {B} with respect to {A} using the following formula: \[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
-@@ -1534,6 +1603,7 @@ We then compute the corresponding rotation matrix.X = stewart.J\args.dL; +
X = J\args.dL;Bibliography
@@ -1543,7 +1613,7 @@ We then compute the corresponding rotation matrix.-diff --git a/kinematic-study.org b/kinematic-study.org index 331353c..3951bb2 100644 --- a/kinematic-study.org +++ b/kinematic-study.org @@ -591,22 +591,39 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]]. % % Inputs: % - stewart - With at least the following fields: - % - As [3x6] - The 6 unit vectors for each strut expressed in {A} - % - Ab [3x6] - The 6 position of the joints bi expressed in {A} + % - 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: - % - J [6x6] - The Jacobian Matrix - % - K [6x6] - The Stiffness Matrix - % - C [6x6] - The Compliance Matrix + % - kinematics.J [6x6] - The Jacobian Matrix + % - kinematics.K [6x6] - The Stiffness Matrix + % - kinematics.C [6x6] - The Compliance Matrix #+end_src +*** Check the =stewart= structure elements +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + 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; +#+end_src + + *** Compute Jacobian Matrix :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab - stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)']; + J = [As' , cross(Ab, As)']; #+end_src *** Compute Stiffness Matrix @@ -614,7 +631,7 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]]. :UNNUMBERED: t :END: #+begin_src matlab - stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J; + K = J'*diag(Ki)*J; #+end_src *** Compute Compliance Matrix @@ -622,9 +639,20 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]]. :UNNUMBERED: t :END: #+begin_src matlab - stewart.C = inv(stewart.K); + C = inv(K); #+end_src +*** Populate the =stewart= structure +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + stewart.kinematics.J = J; + stewart.kinematics.K = K; + stewart.kinematics.C = C; +#+end_src + + ** =inverseKinematics=: Compute Inverse Kinematics :PROPERTIES: :header-args:matlab+: :tangle src/inverseKinematics.m @@ -634,41 +662,6 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]]. This Matlab function is accessible [[file:src/inverseKinematics.m][here]]. -*** Function description -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - 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 - % - 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: - % - 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} -#+end_src - -*** Optional Parameters -:PROPERTIES: -:UNNUMBERED: t -:END: -#+begin_src matlab - arguments - stewart - args.AP (3,1) double {mustBeNumeric} = zeros(3,1) - args.ARB (3,3) double {mustBeNumeric} = eye(3) - end -#+end_src - *** Theory :PROPERTIES: :UNNUMBERED: t @@ -694,16 +687,68 @@ Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by: If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation. Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable. +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + 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} +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + end +#+end_src + +*** Check the =stewart= structure elements +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + 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; +#+end_src + + *** Compute :PROPERTIES: :UNNUMBERED: t :END: #+begin_src matlab - Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa)); + 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)); #+end_src #+begin_src matlab - dLi = Li-stewart.l; + dLi = Li-l; #+end_src ** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics @@ -728,7 +773,7 @@ This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]]. % % Inputs: % - stewart - A structure with the following fields - % - J [6x6] - The Jacobian Matrix + % - kinematics.J [6x6] - The Jacobian Matrix % - args - Can have the following fields: % - dL [6x1] - Displacement of each strut [m] % @@ -748,6 +793,15 @@ This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]]. end #+end_src +*** Check the =stewart= structure elements +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') + J = stewart.kinematics.J; +#+end_src + *** Computation :PROPERTIES: :UNNUMBERED: t @@ -756,7 +810,7 @@ From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the position and orientation of {B} with respect to {A} using the following formula: \[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \] #+begin_src matlab - X = stewart.J\args.dL; + X = J\args.dL; #+end_src The position vector corresponds to the first 3 elements. @@ -777,6 +831,7 @@ We then compute the corresponding rotation matrix. 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 + * Bibliography :ignore: bibliographystyle:unsrt bibliography:ref.bib diff --git a/simscape_subsystems/Fixed_Based.slx b/simscape_subsystems/Fixed_Based.slx index b87fcc9..7efce89 100644 Binary files a/simscape_subsystems/Fixed_Based.slx and b/simscape_subsystems/Fixed_Based.slx differ diff --git a/simscape_subsystems/Mobile_Platform.slx b/simscape_subsystems/Mobile_Platform.slx index f3785f1..2e7ba00 100644 Binary files a/simscape_subsystems/Mobile_Platform.slx and b/simscape_subsystems/Mobile_Platform.slx differ diff --git a/simscape_subsystems/Stewart_Platform.slx b/simscape_subsystems/Stewart_Platform.slx index e3670c2..eab4181 100644 Binary files a/simscape_subsystems/Stewart_Platform.slx and b/simscape_subsystems/Stewart_Platform.slx differ diff --git a/simscape_subsystems/stewart_strut.slx b/simscape_subsystems/stewart_strut.slx index 33cc196..37179d4 100644 Binary files a/simscape_subsystems/stewart_strut.slx and b/simscape_subsystems/stewart_strut.slx differ diff --git a/simscape_subsystems/z_axis_geophone.slx b/simscape_subsystems/z_axis_geophone.slx index 81940c8..35ed473 100644 Binary files a/simscape_subsystems/z_axis_geophone.slx and b/simscape_subsystems/z_axis_geophone.slx differ diff --git a/simulink/stewart_active_damping.slx b/simulink/stewart_active_damping.slx index 07355f6..afa908c 100644 Binary files a/simulink/stewart_active_damping.slx and b/simulink/stewart_active_damping.slx differ diff --git a/src/computeJacobian.m b/src/computeJacobian.m index 80200a0..e4a37a8 100644 --- a/src/computeJacobian.m +++ b/src/computeJacobian.m @@ -5,17 +5,31 @@ function [stewart] = computeJacobian(stewart) % % Inputs: % - stewart - With at least the following fields: -% - As [3x6] - The 6 unit vectors for each strut expressed in {A} -% - Ab [3x6] - The 6 position of the joints bi expressed in {A} +% - 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: -% - J [6x6] - The Jacobian Matrix -% - K [6x6] - The Stiffness Matrix -% - C [6x6] - The Compliance Matrix +% - kinematics.J [6x6] - The Jacobian Matrix +% - kinematics.K [6x6] - The Stiffness Matrix +% - kinematics.C [6x6] - The Compliance Matrix -stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)']; +assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As') +As = stewart.geometry.As; -stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J; +assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab') +Ab = stewart.geometry.Ab; -stewart.C = inv(stewart.K); +assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K') +Ki = stewart.actuators.K; + +J = [As' , cross(Ab, As)']; + +K = J'*diag(Ki)*J; + +C = inv(K); + +stewart.kinematics.J = J; +stewart.kinematics.K = K; +stewart.kinematics.C = C; diff --git a/src/computeJointsPose.m b/src/computeJointsPose.m index 5b5754e..3e31760 100644 --- a/src/computeJointsPose.m +++ b/src/computeJointsPose.m @@ -5,43 +5,74 @@ function [stewart] = computeJointsPose(stewart) % % Inputs: % - stewart - A structure with the following fields -% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} -% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} -% - FO_A [3x1] - Position of {A} with respect to {F} -% - MO_B [3x1] - Position of {B} with respect to {M} -% - FO_M [3x1] - Position of {M} with respect to {F} +% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} +% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} +% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} +% - geometry.FO_M [3x1] - Position of {M} with respect to {F} % % Outputs: % - stewart - A structure with the following added fields -% - Aa [3x6] - The i'th column is the position of ai with respect to {A} -% - Ab [3x6] - The i'th column is the position of bi with respect to {A} -% - Ba [3x6] - The i'th column is the position of ai with respect to {B} -% - Bb [3x6] - The i'th column is the position of bi with respect to {B} -% - l [6x1] - The i'th element is the initial length of strut i -% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A} -% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} -% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} -% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} +% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A} +% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A} +% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B} +% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B} +% - geometry.l [6x1] - The i'th element is the initial length of strut i +% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A} +% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} +% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut +% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut +% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} +% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} -stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]); -stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]); +assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') +Fa = stewart.platform_F.Fa; -stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]); -stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]); +assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb') +Mb = stewart.platform_M.Mb; -stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As +assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A') +FO_A = stewart.platform_F.FO_A; -stewart.l = vecnorm(stewart.Ab - stewart.Aa)'; +assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B') +MO_B = stewart.platform_M.MO_B; -stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba); +assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M') +FO_M = stewart.geometry.FO_M; -stewart.FRa = zeros(3,3,6); -stewart.MRb = zeros(3,3,6); +Aa = Fa - repmat(FO_A, [1, 6]); +Bb = Mb - repmat(MO_B, [1, 6]); + +Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); +Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]); + +As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As + +l = vecnorm(Ab - Aa)'; + +Bs = (Bb - Ba)./vecnorm(Bb - Ba); + +FRa = zeros(3,3,6); +MRb = zeros(3,3,6); for i = 1:6 - stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)]; - stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i)); + 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)); - stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)]; - stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,i)); + MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)]; + MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i)); end + +stewart.geometry.Aa = Aa; +stewart.geometry.Ab = Ab; +stewart.geometry.Ba = Ba; +stewart.geometry.Bb = Bb; +stewart.geometry.As = As; +stewart.geometry.Bs = Bs; +stewart.geometry.l = l; + +stewart.struts_F.l = l/2; +stewart.struts_M.l = l/2; + +stewart.platform_F.FRa = FRa; +stewart.platform_M.MRb = MRb; diff --git a/src/displayArchitecture.m b/src/displayArchitecture.m index 1d9054c..39de82d 100644 --- a/src/displayArchitecture.m +++ b/src/displayArchitecture.m @@ -27,28 +27,43 @@ arguments 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.frames logical {mustBeNumericOrLogical} = true + args.legs logical {mustBeNumericOrLogical} = true + args.joints logical {mustBeNumericOrLogical} = true + args.labels logical {mustBeNumericOrLogical} = true args.platforms logical {mustBeNumericOrLogical} = true 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; + figure; hold on; -FTa = [eye(3), stewart.FO_A; ... +FTa = [eye(3), FO_A; ... zeros(1,3), 1]; ATb = [args.ARB, args.AP; ... zeros(1,3), 1]; -BTm = [eye(3), -stewart.MO_B; ... +BTm = [eye(3), -MO_B; ... zeros(1,3), 1]; FTm = FTa*ATb*BTm; -d_unit_vector = stewart.H/4; +d_unit_vector = H/4; -d_label = stewart.H/20; +d_label = H/20; Ff = [0, 0, 0]; if args.frames @@ -62,24 +77,22 @@ if args.frames end end -Fa = stewart.FO_A; - if args.frames - quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ... + 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(Fa(1) + d_label, ... - Fa(2) + d_label, ... - Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color); + text(FO_A(1) + d_label, ... + FO_A(2) + d_label, ... + FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color); end end -if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr') +if args.platforms && stewart.platform_F.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal center = [0; 0; 0]; % Center of the circle - radius = stewart.platforms.Fpr; % Radius of the circle [m] + radius = stewart.platform_F.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); @@ -89,14 +102,14 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, end if args.joints - scatter3(stewart.Fa(1,:), ... - stewart.Fa(2,:), ... - stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color); + scatter3(Fa(1,:), ... + Fa(2,:), ... + Fa(3,:), 'MarkerEdgeColor', args.F_color); if args.labels - for i = 1:size(stewart.Fa,2) - text(stewart.Fa(1,i) + d_label, ... - stewart.Fa(2,i), ... - stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color); + 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 @@ -115,7 +128,7 @@ if args.frames end end -FB = stewart.FO_A + args.AP; +FB = FO_A + args.AP; if args.frames FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors @@ -129,11 +142,11 @@ if args.frames end end -if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr') +if args.platforms && stewart.platform_M.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal center = Fm(1:3); % Center of the circle - radius = stewart.platforms.Mpr; % Radius of the circle [m] + radius = stewart.platform_M.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); @@ -143,7 +156,7 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, end if args.joints - Fb = FTm*[stewart.Mb;ones(1,6)]; + Fb = FTm*[Mb;ones(1,6)]; scatter3(Fb(1,:), ... Fb(2,:), ... @@ -160,14 +173,14 @@ end if args.legs for i = 1:6 - plot3([stewart.Fa(1,i), Fb(1,i)], ... - [stewart.Fa(2,i), Fb(2,i)], ... - [stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color); + 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((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ... - (stewart.Fa(2,i)+Fb(2,i))/2, ... - (stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color); + 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 diff --git a/src/forwardKinematicsApprox.m b/src/forwardKinematicsApprox.m index 942b92a..a6f3c23 100644 --- a/src/forwardKinematicsApprox.m +++ b/src/forwardKinematicsApprox.m @@ -6,7 +6,7 @@ function [P, R] = forwardKinematicsApprox(stewart, args) % % Inputs: % - stewart - A structure with the following fields -% - J [6x6] - The Jacobian Matrix +% - kinematics.J [6x6] - The Jacobian Matrix % - args - Can have the following fields: % - dL [6x1] - Displacement of each strut [m] % @@ -19,7 +19,10 @@ arguments args.dL (6,1) double {mustBeNumeric} = zeros(6,1) end -X = stewart.J\args.dL; +assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J') +J = stewart.kinematics.J; + +X = J\args.dL; P = X(1:3); diff --git a/src/generateCubicConfiguration.m b/src/generateCubicConfiguration.m index 920cfdf..596787b 100644 --- a/src/generateCubicConfiguration.m +++ b/src/generateCubicConfiguration.m @@ -5,7 +5,7 @@ function [stewart] = generateCubicConfiguration(stewart, args) % % Inputs: % - stewart - A structure with the following fields -% - H [1x1] - Total height of the platform [m] +% - geometry.H [1x1] - Total height of the platform [m] % - args - Can have the following fields: % - Hc [1x1] - Height of the "useful" part of the cube [m] % - FOc [1x1] - Height of the center of the cube with respect to {F} [m] @@ -14,8 +14,8 @@ function [stewart] = generateCubicConfiguration(stewart, args) % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} -% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - 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 @@ -25,6 +25,9 @@ arguments args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3 end +assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H') +H = stewart.geometry.H; + sx = [ 2; -1; -1]; sy = [ 0; 1; -1]; sz = [ 1; 1; 1]; @@ -40,5 +43,8 @@ CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresp CSi = (CCm - CCf)./vecnorm(CCm - CCf); -stewart.Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; -stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; +Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; +Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; + +stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; diff --git a/src/generateGeneralConfiguration.m b/src/generateGeneralConfiguration.m index ce9de8b..fbb570d 100644 --- a/src/generateGeneralConfiguration.m +++ b/src/generateGeneralConfiguration.m @@ -14,8 +14,8 @@ function [stewart] = generateGeneralConfiguration(stewart, args) % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} -% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - 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 @@ -27,10 +27,13 @@ arguments args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); end -stewart.Fa = zeros(3,6); -stewart.Mb = zeros(3,6); +Fa = zeros(3,6); +Mb = zeros(3,6); for i = 1:6 - stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; - stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; + Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; + Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; end + +stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; diff --git a/src/initializeAmplifiedStrutDynamics.m b/src/initializeAmplifiedStrutDynamics.m index be9d3bc..a22bdb0 100644 --- a/src/initializeAmplifiedStrutDynamics.m +++ b/src/initializeAmplifiedStrutDynamics.m @@ -5,35 +5,39 @@ function [stewart] = initializeAmplifiedStrutDynamics(stewart, args) % % Inputs: % - args - Structure with the following fields: -% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] -% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] +% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] +% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] +% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m] +% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)] % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Ki [6x1] - Total Stiffness of each strut [N/m] -% - Ci [6x1] - Total Damping of each strut [N/(m/s)] -% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] -% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] +% - actuators.type = 2 +% - actuators.K [6x1] - Total Stiffness of each strut [N/m] +% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)] +% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] +% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] +% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] +% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] arguments stewart - args.Kdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1) - args.Cdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) - args.Ksi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1) - args.Csi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1) + args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1) + args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) end -stewart.Ksi = args.Ksi; -stewart.Csi = args.Csi; +K = args.Ka + args.Kr; +C = args.Ca + args.Cr; -stewart.Kdi = args.Kdi; -stewart.Cdi = args.Cdi; +stewart.actuators.type = 2; -stewart.Ki = args.Ksi + args.Kdi; -stewart.Ci = args.Csi + args.Cdi; +stewart.actuators.Ka = args.Ka; +stewart.actuators.Ca = args.Ca; -stewart.actuator_type = 2; +stewart.actuators.Kr = args.Kr; +stewart.actuators.Cr = args.Cr; + +stewart.actuators.K = K; +stewart.actuators.C = K; diff --git a/src/initializeCylindricalPlatforms.m b/src/initializeCylindricalPlatforms.m index 6f93b1f..0b86fc1 100644 --- a/src/initializeCylindricalPlatforms.m +++ b/src/initializeCylindricalPlatforms.m @@ -14,15 +14,17 @@ function [stewart] = initializeCylindricalPlatforms(stewart, args) % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - platforms [struct] - structure with the following fields: -% - Fpm [1x1] - Fixed Platform Mass [kg] -% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2] -% - Fph [1x1] - Fixed Platform Height [m] -% - Fpr [1x1] - Fixed Platform Radius [m] -% - Mpm [1x1] - Mobile Platform Mass [kg] -% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2] -% - Mph [1x1] - Mobile Platform Height [m] -% - Mpr [1x1] - Mobile Platform Radius [m] +% - platform_F [struct] - structure with the following fields: +% - type = 1 +% - M [1x1] - Fixed Platform Mass [kg] +% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2] +% - H [1x1] - Fixed Platform Height [m] +% - R [1x1] - Fixed Platform Radius [m] +% - platform_M [struct] - structure with the following fields: +% - M [1x1] - Mobile Platform Mass [kg] +% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2] +% - H [1x1] - Mobile Platform Height [m] +% - R [1x1] - Mobile Platform Radius [m] arguments stewart @@ -34,20 +36,24 @@ arguments args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 end -platforms = struct(); +I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/2 *args.Fpm * args.Fpr^2]); -platforms.Fpm = args.Fpm; -platforms.Fph = args.Fph; -platforms.Fpr = args.Fpr; -platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/2 * platforms.Fpm * platforms.Fpr^2]); +I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/2 *args.Mpm * args.Mpr^2]); -platforms.Mpm = args.Mpm; -platforms.Mph = args.Mph; -platforms.Mpr = args.Mpr; -platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/2 * platforms.Mpm * platforms.Mpr^2]); +stewart.platform_F.type = 1; -stewart.platforms = platforms; +stewart.platform_F.I = I_F; +stewart.platform_F.M = args.Fpm; +stewart.platform_F.R = args.Fpr; +stewart.platform_F.H = args.Fph; + +stewart.platform_M.type = 1; + +stewart.platform_M.I = I_M; +stewart.platform_M.M = args.Mpm; +stewart.platform_M.R = args.Mpr; +stewart.platform_M.H = args.Mph; diff --git a/src/initializeCylindricalStruts.m b/src/initializeCylindricalStruts.m index a83d0c5..2bb7257 100644 --- a/src/initializeCylindricalStruts.m +++ b/src/initializeCylindricalStruts.m @@ -14,15 +14,16 @@ function [stewart] = initializeCylindricalStruts(stewart, args) % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - struts [struct] - structure with the following fields: -% - Fsm [6x1] - Mass of the Fixed part of the struts [kg] -% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] -% - Msm [6x1] - Mass of the Mobile part of the struts [kg] -% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] -% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m] -% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m] -% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m] -% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m] +% - 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 @@ -34,25 +35,37 @@ arguments args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 end -struts = struct(); +Fsm = ones(6,1).*args.Fsm; +Fsh = ones(6,1).*args.Fsh; +Fsr = ones(6,1).*args.Fsr; -struts.Fsm = ones(6,1).*args.Fsm; -struts.Msm = ones(6,1).*args.Msm; +Msm = ones(6,1).*args.Msm; +Msh = ones(6,1).*args.Msh; +Msr = ones(6,1).*args.Msr; -struts.Fsh = ones(6,1).*args.Fsh; -struts.Fsr = ones(6,1).*args.Fsr; -struts.Msh = ones(6,1).*args.Msh; -struts.Msr = ones(6,1).*args.Msr; +I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut +I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut -struts.Fsi = zeros(3, 3, 6); -struts.Msi = zeros(3, 3, 6); for i = 1:6 - struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/2 * struts.Fsm(i) * struts.Fsr(i)^2]); - struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/2 * struts.Msm(i) * struts.Msr(i)^2]); + I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/2 * Fsm(i) * Fsr(i)^2]); + + I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/2 * Msm(i) * Msr(i)^2]); end -stewart.struts = struts; +stewart.struts_M.type = 1; + +stewart.struts_M.I = I_M; +stewart.struts_M.M = Msm; +stewart.struts_M.R = Msr; +stewart.struts_M.H = Msh; + +stewart.struts_F.type = 1; + +stewart.struts_F.I = I_F; +stewart.struts_F.M = Fsm; +stewart.struts_F.R = Fsr; +stewart.struts_F.H = Fsh; diff --git a/src/initializeFramesPositions.m b/src/initializeFramesPositions.m index efb99a5..0a136ef 100644 --- a/src/initializeFramesPositions.m +++ b/src/initializeFramesPositions.m @@ -1,4 +1,4 @@ -function [stewart] = initializeFramesPositions(args) +function [stewart] = initializeFramesPositions(stewart, args) % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} % % Syntax: [stewart] = initializeFramesPositions(args) @@ -10,22 +10,26 @@ function [stewart] = initializeFramesPositions(args) % % Outputs: % - stewart - A structure with the following fields: -% - H [1x1] - Total Height of the Stewart Platform [m] -% - FO_M [3x1] - Position of {M} with respect to {F} [m] -% - MO_B [3x1] - Position of {B} with respect to {M} [m] -% - FO_A [3x1] - Position of {A} with respect to {F} [m] +% - 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 -stewart = struct(); +H = args.H; % Total Height of the Stewart Platform [m] -stewart.H = args.H; % Total Height of the Stewart Platform [m] +FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m] -stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m] +MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] -stewart.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.FO_A = stewart.MO_B + stewart.FO_M; % Position of {A} with respect to {F} [m] +stewart.geometry.H = H; +stewart.geometry.FO_M = FO_M; +stewart.platform_M.MO_B = MO_B; +stewart.platform_F.FO_A = FO_A; diff --git a/src/initializeInertialSensor.m b/src/initializeInertialSensor.m new file mode 100644 index 0000000..a9509e4 --- /dev/null +++ b/src/initializeInertialSensor.m @@ -0,0 +1,48 @@ +function [stewart] = initializeInertialSensor(stewart, args) +% initializeInertialSensor - Initialize the inertial sensor in each strut +% +% Syntax: [stewart] = initializeInertialSensor(args) +% +% Inputs: +% - args - Structure with the following fields: +% - type - 'geophone', 'accelerometer', 'none' +% - mass [1x1] - Weight of the inertial mass [kg] +% - freq [1x1] - Cutoff frequency [Hz] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - stewart.sensors.inertial +% - type - 1 (geophone), 2 (accelerometer), 3 (none) +% - K [1x1] - Stiffness [N/m] +% - C [1x1] - Damping [N/(m/s)] +% - M [1x1] - Inertial Mass [kg] +% - G [1x1] - Gain + +arguments + stewart + args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none' + args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2 + args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3 +end + +sensor = struct(); + +switch args.type + case 'geophone' + sensor.type = 1; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + case 'accelerometer' + sensor.type = 2; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + sensor.G = -sensor.K/sensor.M; + case 'none' + sensor.type = 3; +end + +stewart.sensors.inertial = sensor; diff --git a/src/initializeJointDynamics.m b/src/initializeJointDynamics.m index 62f23e5..3b3733a 100644 --- a/src/initializeJointDynamics.m +++ b/src/initializeJointDynamics.m @@ -5,46 +5,88 @@ function [stewart] = initializeJointDynamics(stewart, args) % % Inputs: % - args - Structure with the following fields: -% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad] -% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)] -% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad] -% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)] -% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad] -% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)] -% - disable [boolean] - Sets all the stiffness/damping to zero +% - type_F - 'universal', 'spherical', 'univesal_p', 'spherical_p' +% - type_M - 'universal', 'spherical', 'univesal_p', 'spherical_p' +% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad] +% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad] +% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)] +% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)] +% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad] +% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad] +% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)] +% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)] % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Ksbi [6x1] - Bending (Rx, Ry) Stiffness for each top Spherical joints [(N.m)/rad] -% - Csbi [6x1] - Bending (Rx, Ry) Damping of each top Spherical joint [(N.m)/(rad/s)] -% - Ksti [6x1] - Torsion (Rz) Stiffness for each top Spherical joints [(N.m)/rad] -% - Csti [6x1] - Torsion (Rz) Damping of each top Spherical joint [(N.m)/(rad/s)] -% - Kubi [6x1] - Bending (Rx, Ry) Stiffness for each bottom Universal joints [(N.m)/rad] -% - Cubi [6x1] - Bending (Rx, Ry) Damping of each bottom Universal joint [(N.m)/(rad/s)] +% - stewart.joints_F and stewart.joints_M: +% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect) +% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m] +% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad] +% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad] +% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)] +% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)] +% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)] arguments stewart - args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) - args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) - args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) - args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) - args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.disable logical {mustBeNumericOrLogical} = false + args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'universal' + args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'spherical' + args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) + args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) + args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) + args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) + args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) end -if args.disable - stewart.Ksbi = zeros(6,1); - stewart.Csbi = zeros(6,1); - stewart.Ksti = zeros(6,1); - stewart.Csti = zeros(6,1); - stewart.Kubi = zeros(6,1); - stewart.Cubi = zeros(6,1); -else - stewart.Ksbi = args.Ksbi; - stewart.Csbi = args.Csbi; - stewart.Ksti = args.Ksti; - stewart.Csti = args.Csti; - stewart.Kubi = args.Kubi; - stewart.Cubi = args.Cubi; +switch args.type_F + case 'universal' + stewart.joints_F.type = 1; + case 'spherical' + stewart.joints_F.type = 2; + case 'universal_p' + stewart.joints_F.type = 3; + case 'spherical_p' + stewart.joints_F.type = 4; end + +switch args.type_M + case 'universal' + stewart.joints_M.type = 1; + case 'spherical' + stewart.joints_M.type = 2; + case 'universal_p' + stewart.joints_M.type = 3; + case 'spherical_p' + stewart.joints_M.type = 4; +end + +stewart.joints_M.Kx = zeros(6,1); +stewart.joints_M.Ky = zeros(6,1); +stewart.joints_M.Kz = zeros(6,1); + +stewart.joints_F.Kx = zeros(6,1); +stewart.joints_F.Ky = zeros(6,1); +stewart.joints_F.Kz = zeros(6,1); + +stewart.joints_M.Cx = zeros(6,1); +stewart.joints_M.Cy = zeros(6,1); +stewart.joints_M.Cz = zeros(6,1); + +stewart.joints_F.Cx = zeros(6,1); +stewart.joints_F.Cy = zeros(6,1); +stewart.joints_F.Cz = zeros(6,1); + +stewart.joints_M.Kf = args.Kf_M; +stewart.joints_M.Kt = args.Kf_M; + +stewart.joints_F.Kf = args.Kf_F; +stewart.joints_F.Kt = args.Kf_F; + +stewart.joints_M.Cf = args.Cf_M; +stewart.joints_M.Ct = args.Cf_M; + +stewart.joints_F.Cf = args.Cf_F; +stewart.joints_F.Ct = args.Cf_F; diff --git a/src/initializeStewartPlatform.m b/src/initializeStewartPlatform.m new file mode 100644 index 0000000..0545b4d --- /dev/null +++ b/src/initializeStewartPlatform.m @@ -0,0 +1,31 @@ +function [stewart] = initializeStewartPlatform() +% initializeStewartPlatform - Initialize the stewart structure +% +% Syntax: [stewart] = initializeStewartPlatform(args) +% +% Outputs: +% - stewart - A structure with the following sub-structures: +% - platform_F - +% - platform_M - +% - joints_F - +% - joints_M - +% - struts_F - +% - struts_M - +% - actuators - +% - geometry - +% - properties - + +stewart = struct(); +stewart.platform_F = struct(); +stewart.platform_M = struct(); +stewart.joints_F = struct(); +stewart.joints_M = struct(); +stewart.struts_F = struct(); +stewart.struts_M = struct(); +stewart.actuators = struct(); +stewart.sensors = struct(); +stewart.sensors.inertial = struct(); +stewart.sensors.force = struct(); +stewart.sensors.relative = struct(); +stewart.geometry = struct(); +stewart.kinematics = struct(); diff --git a/src/initializeStewartPose.m b/src/initializeStewartPose.m index a32f8eb..325b501 100644 --- a/src/initializeStewartPose.m +++ b/src/initializeStewartPose.m @@ -14,7 +14,7 @@ function [stewart] = initializeStewartPose(stewart, args) % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - 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} +% - 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 @@ -24,4 +24,4 @@ end [Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); -stewart.dLi = dLi; +stewart.actuators.Leq = dLi; diff --git a/src/initializeStrutDynamics.m b/src/initializeStrutDynamics.m index 8776bf8..c7b9a52 100644 --- a/src/initializeStrutDynamics.m +++ b/src/initializeStrutDynamics.m @@ -5,21 +5,22 @@ function [stewart] = initializeStrutDynamics(stewart, args) % % Inputs: % - args - Structure with the following fields: -% - Ki [6x1] - Stiffness of each strut [N/m] -% - Ci [6x1] - Damping of each strut [N/(m/s)] +% - 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: -% - Ki [6x1] - Stiffness of each strut [N/m] -% - Ci [6x1] - Damping of each strut [N/(m/s)] +% - 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.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1) - args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1) + args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) end -stewart.Ki = args.Ki; -stewart.Ci = args.Ci; +stewart.actuators.type = 1; -stewart.actuator_type = 1; +stewart.actuators.K = args.K; +stewart.actuators.C = args.C; diff --git a/src/inverseKinematics.m b/src/inverseKinematics.m index a214e1f..c1c0618 100644 --- a/src/inverseKinematics.m +++ b/src/inverseKinematics.m @@ -5,8 +5,9 @@ function [Li, dLi] = inverseKinematics(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} +% - 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} @@ -21,6 +22,15 @@ arguments args.ARB (3,3) double {mustBeNumeric} = eye(3) end -Li = sqrt(args.AP'*args.AP + diag(stewart.Bb'*stewart.Bb) + diag(stewart.Aa'*stewart.Aa) - (2*args.AP'*stewart.Aa)' + (2*args.AP'*(args.ARB*stewart.Bb))' - diag(2*(args.ARB*stewart.Bb)'*stewart.Aa)); +assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa') +Aa = stewart.geometry.Aa; -dLi = Li-stewart.l; +assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb') +Bb = stewart.geometry.Bb; + +assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l') +l = stewart.geometry.l; + +Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa)); + +dLi = Li-l; diff --git a/stewart-architecture.html b/stewart-architecture.html index 6c05161..c858337 100644 --- a/stewart-architecture.html +++ b/stewart-architecture.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - +Created: 2020-01-29 mer. 20:22
+Created: 2020-02-11 mar. 15:10
Stewart Platform - Definition of the Architecture @@ -268,113 +268,137 @@ for the JavaScript code in this tag.Table of Contents
@@ -718,15 +743,23 @@ stewart = initializeCylindricalStruts(stewart);-
-
- 1. Definition of the Stewart Platform Geometry +
- 1. Definition of the Stewart Platform Geometry -
- 2. Definition of the Inertia and geometry of the Fixed base, Mobile platform and Struts +
- 2. Definition of the Inertia and geometry of the Fixed base, Mobile platform and Struts -
- 3. Definition of the stiffness and damping of the joints +
- 3. Definition of the stiffness and damping of the joints -
- 4. Summary of the Initialization Procedure and Matlab Example +
- 4. Summary of the Initialization Procedure and Matlab Example -
- 5. Functions +
- 5. Functions
-
-
- 5.1.
initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M} + - 5.1.
initializeStewartPlatform: Initialize the Stewart Platform structure
- - 5.2.
generateGeneralConfiguration: Generate a Very General Configuration + - 5.2.
initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}
- - 5.3.
computeJointsPose: Compute the Pose of the Joints + - 5.3.
generateGeneralConfiguration: Generate a Very General Configuration
- - 5.4.
initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose + - 5.4.
computeJointsPose: Compute the Pose of the Joints-
-
- Function description -
- Optional Parameters -
- Use the Inverse Kinematic function +
- Documentation +
- Function description +
- Check the
stewartstructure elements
+ - Compute the position of the Joints +
- Compute the strut length and orientation +
- Compute the orientation of the Joints +
- Populate the
stewartstructure
- - 5.5.
initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms + - 5.5.
initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose
- - 5.6.
initializeCylindricalStruts: Define the inertia of cylindrical struts + - 5.6.
initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms
- - 5.7.
initializeStrutDynamics: Add Stiffness and Damping properties of each strut + - 5.7.
initializeCylindricalStruts: Define the inertia of cylindrical struts
- - 5.8.
initializeAmplifiedStrutDynamics: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator + - 5.8.
initializeStrutDynamics: Add Stiffness and Damping properties of each strut
- - 5.9.
initializeJointDynamics: Add Stiffness and Damping properties for spherical joints + - 5.9.
initializeAmplifiedStrutDynamics: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator
- - 5.10.
displayArchitecture: 3D plot of the Stewart platform architecture + - 5.10.
initializeJointDynamics: Add Stiffness and Damping properties for spherical joints-
-
- Function description -
- Optional Parameters -
- Figure Creation, Frames and Homogeneous transformations -
- Fixed Base elements -
- Mobile Platform elements -
- Legs -
- 5.10.1. Figure parameters +
- Function description +
- Optional Parameters +
- Add Actuator Type +
- Add Stiffness and Damping in Translation of each strut +
- Add Stiffness and Damping in Rotation of each strut +
+ - 5.11.
initializeInertialSensor: Initialize the inertial sensor in each strut + +
+ - 5.12.
displayArchitecture: 3D plot of the Stewart platform architecture +
stewart = initializeStewartPlatform(); +stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); stewart = generateGeneralConfiguration(stewart); stewart = computeJointsPose(stewart); -stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3)); +stewart = initializeStewartPose(stewart, 'AP', [0;0;0], 'ARB', eye(3));
- 5.1.
-Finally, initialize the strut stiffness and damping properties. +We initialize the strut stiffness and damping properties.
-+stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); +
stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e2*ones(6,1)); stewart = initializeAmplifiedStrutDynamics(stewart); stewart = initializeJointDynamics(stewart);
+And finally the inertial sensors included in each strut. +
+++stewart = initializeInertialSensor(stewart, 'type', 'none'); +
+The obtained
@@ -740,7 +773,7 @@ The functionstewartMatlab structure contains all the information for analysis of the Stewart platform and for simulations using Simscape.displayArchitecturecan be used to display the current+-
Figure 5: Display of the current Stewart platform architecture (png, pdf)
@@ -748,7 +781,7 @@ The functiondisplayArchitecturecan be used to display the currentThere are many options to show or hides elements such as labels and frames. -The documentation of the function is available here. +The documentation of the function is available here.
@@ -780,7 +813,7 @@ view([0 -1 0]);
+--5 Functions
++5 Functions
--5.1
+ +initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}+5.1
-initializeStewartPlatform: Initialize the Stewart Platform structure-Documentation
-++Documentation
+-+-
Figure 7: Definition of the position of the frames
@@ -820,14 +854,87 @@ This Matlab function is accessible her-Function description
-++ +++ +Function description
++-+function [stewart] = initializeFramesPositions(args) +
function [stewart] = initializeStewartPlatform() +% initializeStewartPlatform - Initialize the stewart structure +% +% Syntax: [stewart] = initializeStewartPlatform(args) +% +% Outputs: +% - stewart - A structure with the following sub-structures: +% - platform_F - +% - platform_M - +% - joints_F - +% - joints_M - +% - struts_F - +% - struts_M - +% - actuators - +% - geometry - +% - properties - +
+++Initialize the Stewart structure
+++++stewart = struct(); +stewart.platform_F = struct(); +stewart.platform_M = struct(); +stewart.joints_F = struct(); +stewart.joints_M = struct(); +stewart.struts_F = struct(); +stewart.struts_M = struct(); +stewart.actuators = struct(); +stewart.sensors = struct(); +stewart.sensors.inertial = struct(); +stewart.sensors.force = struct(); +stewart.sensors.relative = struct(); +stewart.geometry = struct(); +stewart.kinematics = struct(); +
++5.2
+initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}+ + ++ ++This Matlab function is accessible here. +
+++ +Documentation
++ ++++
+
+Figure 8: Definition of the position of the frames
++-Function description
+++function [stewart] = initializeFramesPositions(stewart, args) % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} % -% Syntax: [stewart] = initializeFramesPositions(args) +% Syntax: [stewart] = initializeFramesPositions(stewart, args) % % Inputs: % - args - Can have the following fields: @@ -836,20 +943,21 @@ This Matlab function is accessible her % % Outputs: % - stewart - A structure with the following fields: -% - H [1x1] - Total Height of the Stewart Platform [m] -% - FO_M [3x1] - Position of {M} with respect to {F} [m] -% - MO_B [3x1] - Position of {B} with respect to {M} [m] -% - FO_A [3x1] - Position of {A} with respect to {F} [m] +% - 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]
-Optional Parameters
-++Optional Parameters
+-arguments + stewart args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 args.MO_B (1,1) double {mustBeNumeric} = 50e-3 end @@ -858,38 +966,41 @@ This Matlab function is accessible her-Initialize the Stewart structure
-++-Compute the position of each frame
+-stewart = struct(); +
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]
-Compute the position of each frame
-+-+Populate the
+stewartstructure-stewart.H = args.H; % Total Height of the Stewart Platform [m] - -stewart.FO_M = [0; 0; stewart.H]; % Position of {M} with respect to {F} [m] - -stewart.MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m] - -stewart.FO_A = stewart.MO_B + stewart.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;
-5.2
-generateGeneralConfiguration: Generate a Very General Configuration++5.3
+ -generateGeneralConfiguration: Generate a Very General Configuration-Documentation
-++Documentation
+-Joints are positions on a circle centered with the Z axis of {F} and {M} and at a chosen distance from {F} and {M}. -The radius of the circles can be chosen as well as the angles where the joints are located (see Figure 8). +The radius of the circles can be chosen as well as the angles where the joints are located (see Figure 9).
-+
-
Figure 8: Position of the joints
+Figure 9: Position of the joints
-Function description
-++-Function description
+function [stewart] = generateGeneralConfiguration(stewart, args) % generateGeneralConfiguration - Generate a Very General Configuration @@ -934,16 +1045,16 @@ The radius of the circles can be chosen as well as the angles where the joints a % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} -% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - 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}
-Optional Parameters
-++Optional Parameters
+-arguments stewart @@ -959,31 +1070,42 @@ The radius of the circles can be chosen as well as the angles where the joints a-Compute the pose
-+-++ +Compute the pose
+-stewart.Fa = zeros(3,6); -stewart.Mb = zeros(3,6); +
Fa = zeros(3,6); +Mb = zeros(3,6);
for i = 1:6 - stewart.Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH]; - stewart.Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH]; + 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
+Populate the
+stewartstructure++++stewart.platform_F.Fa = Fa; +stewart.platform_M.Mb = Mb; +
+-5.3
-computeJointsPose: Compute the Pose of the Joints++5.4
+computeJointsPose: Compute the Pose of the Joints-@@ -991,21 +1113,21 @@ This Matlab function is accessible here.
-Documentation
-++Documentation
+--+
-
Figure 9: Position and orientation of the struts
+Figure 10: Position and orientation of the struts
-Function description
-++-Function description
+function [stewart] = computeJointsPose(stewart) % computeJointsPose - @@ -1014,84 +1136,131 @@ This Matlab function is accessible here. % % Inputs: % - stewart - A structure with the following fields -% - Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} -% - Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} -% - FO_A [3x1] - Position of {A} with respect to {F} -% - MO_B [3x1] - Position of {B} with respect to {M} -% - FO_M [3x1] - Position of {M} with respect to {F} +% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F} +% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M} +% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} +% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} +% - geometry.FO_M [3x1] - Position of {M} with respect to {F} % % Outputs: % - stewart - A structure with the following added fields -% - Aa [3x6] - The i'th column is the position of ai with respect to {A} -% - Ab [3x6] - The i'th column is the position of bi with respect to {A} -% - Ba [3x6] - The i'th column is the position of ai with respect to {B} -% - Bb [3x6] - The i'th column is the position of bi with respect to {B} -% - l [6x1] - The i'th element is the initial length of strut i -% - As [3x6] - The i'th column is the unit vector of strut i expressed in {A} -% - Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} -% - FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} -% - MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M} +% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A} +% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A} +% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B} +% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B} +% - geometry.l [6x1] - The i'th element is the initial length of strut i +% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A} +% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B} +% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut +% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut +% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F} +% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
-Compute the position of the Joints
-++-Check the
+stewartstructure elements-stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]); -stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]); +
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa') +Fa = stewart.platform_F.Fa; -stewart.Ab = stewart.Bb - repmat(-stewart.MO_B-stewart.FO_M+stewart.FO_A, [1, 6]); -stewart.Ba = stewart.Aa - repmat( stewart.MO_B+stewart.FO_M-stewart.FO_A, [1, 6]); +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;
-Compute the strut length and orientation
-++-Compute the position of the Joints
+-- -stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As +
Aa = Fa - repmat(FO_A, [1, 6]); +Bb = Mb - repmat(MO_B, [1, 6]); -stewart.l = vecnorm(stewart.Ab - stewart.Aa)'; -
--stewart.Bs = (stewart.Bb - stewart.Ba)./vecnorm(stewart.Bb - stewart.Ba); +Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]); +Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
-Compute the orientation of the Joints
-+-++ +Compute the strut length and orientation
++-+ +stewart.FRa = zeros(3,3,6); -stewart.MRb = zeros(3,3,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); +
+++ +Compute the orientation of the Joints
+++FRa = zeros(3,3,6); +MRb = zeros(3,3,6); for i = 1:6 - stewart.FRa(:,:,i) = [cross([0;1;0], stewart.As(:,i)) , cross(stewart.As(:,i), cross([0;1;0], stewart.As(:,i))) , stewart.As(:,i)]; - stewart.FRa(:,:,i) = stewart.FRa(:,:,i)./vecnorm(stewart.FRa(:,:,i)); + 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)); - stewart.MRb(:,:,i) = [cross([0;1;0], stewart.Bs(:,i)) , cross(stewart.Bs(:,i), cross([0;1;0], stewart.Bs(:,i))) , stewart.Bs(:,i)]; - stewart.MRb(:,:,i) = stewart.MRb(:,:,i)./vecnorm(stewart.MRb(:,:,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
+Populate the
+stewartstructure++++stewart.geometry.Aa = Aa; +stewart.geometry.Ab = Ab; +stewart.geometry.Ba = Ba; +stewart.geometry.Bb = Bb; +stewart.geometry.As = As; +stewart.geometry.Bs = Bs; +stewart.geometry.l = l; + +stewart.struts_F.l = l/2; +stewart.struts_M.l = l/2; + +stewart.platform_F.FRa = FRa; +stewart.platform_M.MRb = MRb; +
+-5.4
-initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose++5.5
+initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose-@@ -1099,9 +1268,9 @@ This Matlab function is accessible here
-Function description
-++-Function description
+function [stewart] = initializeStewartPose(stewart, args) % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose @@ -1119,15 +1288,15 @@ This Matlab function is accessible here% % Outputs: % - stewart - updated Stewart structure with the added fields: -% - 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} +% - 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}
-Optional Parameters
-++Optional Parameters
+-arguments stewart @@ -1139,24 +1308,32 @@ This Matlab function is accessible here-Use the Inverse Kinematic function
-+-+-stewart.dLi = dLi; +Use the Inverse Kinematic function
+++[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); +
++Populate the
+stewartstructure++stewart.actuators.Leq = dLi;
-5.5
-initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms++5.6
+initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms@@ -1164,9 +1341,9 @@ This Matlab function is accessible -
Function description
-++-Function description
+function [stewart] = initializeCylindricalPlatforms(stewart, args) % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms @@ -1184,23 +1361,25 @@ This Matlab function is accessible % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - platforms [struct] - structure with the following fields: -% - Fpm [1x1] - Fixed Platform Mass [kg] -% - Msi [3x3] - Mobile Platform Inertia matrix [kg*m^2] -% - Fph [1x1] - Fixed Platform Height [m] -% - Fpr [1x1] - Fixed Platform Radius [m] -% - Mpm [1x1] - Mobile Platform Mass [kg] -% - Fsi [3x3] - Fixed Platform Inertia matrix [kg*m^2] -% - Mph [1x1] - Mobile Platform Height [m] -% - Mpr [1x1] - Mobile Platform Radius [m] +% - platform_F [struct] - structure with the following fields: +% - type = 1 +% - M [1x1] - Fixed Platform Mass [kg] +% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2] +% - H [1x1] - Fixed Platform Height [m] +% - R [1x1] - Fixed Platform Radius [m] +% - platform_M [struct] - structure with the following fields: +% - M [1x1] - Mobile Platform Mass [kg] +% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2] +% - H [1x1] - Mobile Platform Height [m] +% - R [1x1] - Mobile Platform Radius [m]
-Optional Parameters
-++Optional Parameters
+arguments stewart @@ -1216,46 +1395,56 @@ This Matlab function is accessible -Create the
-platformsstruct++-Compute the Inertia matrices of platforms
+--platforms.Fpm = args.Fpm; -platforms.Fph = args.Fph; -platforms.Fpr = args.Fpr; -platforms.Fpi = diag([1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/12 * platforms.Fpm * (3*platforms.Fpr^2 + platforms.Fph^2), ... - 1/2 * platforms.Fpm * platforms.Fpr^2]); - -platforms.Mpm = args.Mpm; -platforms.Mph = args.Mph; -platforms.Mpr = args.Mpr; -platforms.Mpi = diag([1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/12 * platforms.Mpm * (3*platforms.Mpr^2 + platforms.Mph^2), ... - 1/2 * platforms.Mpm * platforms.Mpr^2]); +platforms = struct(); +
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ... + 1/2 *args.Fpm * args.Fpr^2]); +
++I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ... + 1/2 *args.Mpm * args.Mpr^2]);
-Save the
-platformsstruct+-+Populate the
+stewartstructure-+ +stewart.platforms = platforms; +
stewart.platform_F.type = 1; + +stewart.platform_F.I = I_F; +stewart.platform_F.M = args.Fpm; +stewart.platform_F.R = args.Fpr; +stewart.platform_F.H = args.Fph; +
++stewart.platform_M.type = 1; + +stewart.platform_M.I = I_M; +stewart.platform_M.M = args.Mpm; +stewart.platform_M.R = args.Mpr; +stewart.platform_M.H = args.Mph;
-5.6
-initializeCylindricalStruts: Define the inertia of cylindrical struts++5.7
+initializeCylindricalStruts: Define the inertia of cylindrical struts-@@ -1263,9 +1452,9 @@ This Matlab function is accessible h
-Function description
-++-Function description
+function [stewart] = initializeCylindricalStruts(stewart, args) % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts @@ -1283,23 +1472,24 @@ This Matlab function is accessible h % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - struts [struct] - structure with the following fields: -% - Fsm [6x1] - Mass of the Fixed part of the struts [kg] -% - Fsi [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2] -% - Msm [6x1] - Mass of the Mobile part of the struts [kg] -% - Msi [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2] -% - Fsh [6x1] - Height of cylinder for the Fixed part of the struts [m] -% - Fsr [6x1] - Radius of cylinder for the Fixed part of the struts [m] -% - Msh [6x1] - Height of cylinder for the Mobile part of the struts [m] -% - Msr [6x1] - Radius of cylinder for the Mobile part of the struts [m] +% - 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]
-Optional Parameters
-++Optional Parameters
+-arguments stewart @@ -1315,46 +1505,69 @@ This Matlab function is accessible h-Create the
-strutsstructure+-++ +Compute the properties of the cylindrical struts
++--struts.Fsh = ones(6,1).*args.Fsh; -struts.Fsr = ones(6,1).*args.Fsr; -struts.Msh = ones(6,1).*args.Msh; -struts.Msr = ones(6,1).*args.Msr; +struts = struct(); +
Fsm = ones(6,1).*args.Fsm; +Fsh = ones(6,1).*args.Fsh; +Fsr = ones(6,1).*args.Fsr; -struts.Fsm = ones(6,1).*args.Fsm; -struts.Msm = ones(6,1).*args.Msm; +Msm = ones(6,1).*args.Msm; +Msh = ones(6,1).*args.Msh; +Msr = ones(6,1).*args.Msr; +
+++I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut +I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut -struts.Fsi = zeros(3, 3, 6); -struts.Msi = zeros(3, 3, 6); for i = 1:6 - struts.Fsi(:,:,i) = diag([1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/12 * struts.Fsm(i) * (3*struts.Fsr(i)^2 + struts.Fsh(i)^2), ... - 1/2 * struts.Fsm(i) * struts.Fsr(i)^2]); - struts.Msi(:,:,i) = diag([1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/12 * struts.Msm(i) * (3*struts.Msr(i)^2 + struts.Msh(i)^2), ... - 1/2 * struts.Msm(i) * struts.Msr(i)^2]); + I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ... + 1/2 * Fsm(i) * Fsr(i)^2]); + + I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ... + 1/2 * Msm(i) * Msr(i)^2]); end
+Populate the
+stewartstructure++stewart.struts_M.type = 1; + +stewart.struts_M.I = I_M; +stewart.struts_M.M = Msm; +stewart.struts_M.R = Msr; +stewart.struts_M.H = Msh; +
+-stewart.struts = struts; +
stewart.struts_F.type = 1; + +stewart.struts_F.I = I_F; +stewart.struts_F.M = Fsm; +stewart.struts_F.R = Fsr; +stewart.struts_F.H = Fsh;
-5.7
-initializeStrutDynamics: Add Stiffness and Damping properties of each strut++5.8
+initializeStrutDynamics: Add Stiffness and Damping properties of each strut-@@ -1362,39 +1575,40 @@ This Matlab function is accessible here<
-Documentation
-++Documentation
+-+-
-
Figure 10: Example of a piezoelectric stach actuator (PI)
+Figure 11: Example of a piezoelectric stach actuator (PI)
-A simplistic model of such amplified actuator is shown in Figure 11 where: +A simplistic model of such amplified actuator is shown in Figure 12 where:
-
-
- \(k_{i}\) represent the vertical stiffness of the actuator -
- \(F_{i}\) represents the force applied by the actuator -
- \(F_{m,i}\) represents the total measured force -
- \(v_{m,i}\) represents the absolute velocity of the top part of the actuator -
- \(d_{m,i}\) represents the total relative displacement of the actuator +
- \(K\) represent the vertical stiffness of the actuator +
- \(C\) represent the vertical damping of the actuator +
- \(F\) represents the force applied by the actuator +
- \(F_{m}\) represents the total measured force +
- \(v_{m}\) represents the absolute velocity of the top part of the actuator +
- \(d_{m}\) represents the total relative displacement of the actuator
+
-
Figure 11: Simple model of an Actuator
+Figure 12: Simple model of an Actuator
-Function description
-++-Function description
+function [stewart] = initializeStrutDynamics(stewart, args) % initializeStrutDynamics - Add Stiffness and Damping properties of each strut @@ -1403,51 +1617,52 @@ A simplistic model of such amplified actuator is shown in Figure % % Inputs: % - args - Structure with the following fields: -% - Ki [6x1] - Stiffness of each strut [N/m] -% - Ci [6x1] - Damping of each strut [N/(m/s)] +% - 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: -% - Ki [6x1] - Stiffness of each strut [N/m] -% - Ci [6x1] - Damping of each strut [N/(m/s)] +% - actuators.type = 1 +% - actuators.K [6x1] - Stiffness of each strut [N/m] +% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
-Optional Parameters
-++-Optional Parameters
+arguments stewart - args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1) - args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e6*ones(6,1) + args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) end-Add Stiffness and Damping properties of each strut
-+-+Add Stiffness and Damping properties of each strut
+-stewart.Ki = args.Ki; -stewart.Ci = args.Ci; +
stewart.actuators.type = 1; -stewart.actuator_type = 1; +stewart.actuators.K = args.K; +stewart.actuators.C = args.C;
-5.8
-initializeAmplifiedStrutDynamics: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator++5.9
+initializeAmplifiedStrutDynamics: Add Stiffness and Damping properties of each strut for an amplified piezoelectric actuator@@ -1455,26 +1670,26 @@ This Matlab function is accessible -
Documentation
-++Documentation
+--An amplified piezoelectric actuator is shown in Figure 12. +An amplified piezoelectric actuator is shown in Figure 13.
-+
-
Figure 12: Example of an Amplified piezoelectric actuator with an integrated displacement sensor (Cedrat Technologies)
+Figure 13: Example of an Amplified piezoelectric actuator with an integrated displacement sensor (Cedrat Technologies)
-A simplistic model of such amplified actuator is shown in Figure 13 where: +A simplistic model of such amplified actuator is shown in Figure 14 where:
-
-
- \(k_{p,i}\) represent the vertical stiffness when the piezoelectric stack is removed -
- \(k_{s,i}\) is the vertical stiffness contribution of the piezoelectric stack +
- \(K_{r}\) represent the vertical stiffness when the piezoelectric stack is removed +
- \(K_{a}\) is the vertical stiffness contribution of the piezoelectric stack
- \(F_{i}\) represents the part of the piezoelectric stack that is used as a force actuator
- \(F_{m,i}\) represents the remaining part of the piezoelectric stack that is used as a force sensor
- \(v_{m,i}\) represents the absolute velocity of the top part of the actuator @@ -1482,17 +1697,17 @@ A simplistic model of such amplified actuator is shown in Figure +
-
Figure 13: Model of an amplified actuator
+Figure 14: Model of an amplified actuator
-Function description
-++-Function description
+function [stewart] = initializeAmplifiedStrutDynamics(stewart, args) % initializeAmplifiedStrutDynamics - Add Stiffness and Damping properties of each strut @@ -1501,65 +1716,77 @@ A simplistic model of such amplified actuator is shown in Figure % % Inputs: % - args - Structure with the following fields: -% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] -% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] +% - Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] +% - Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] +% - Kr [6x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m] +% - Cr [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)] % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Ki [6x1] - Total Stiffness of each strut [N/m] -% - Ci [6x1] - Total Damping of each strut [N/(m/s)] -% - Ksi [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] -% - Csi [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] -% - Kdi [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] -% - Cdi [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)] +% - actuators.type = 2 +% - actuators.K [6x1] - Total Stiffness of each strut [N/m] +% - actuators.C [6x1] - Total Damping of each strut [N/(m/s)] +% - actuators.Ka [6x1] - Vertical stiffness contribution of the piezoelectric stack [N/m] +% - actuators.Ca [6x1] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] +% - actuators.Kr [6x1] - Vertical stiffness when the piezoelectric stack is removed [N/m] +% - actuators.Cr [6x1] - Vertical damping when the piezoelectric stack is removed [N/(m/s)]
-Optional Parameters
-++-Optional Parameters
+arguments stewart - args.Kdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1) - args.Cdi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) - args.Ksi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1) - args.Csi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Kr (6,1) double {mustBeNumeric, mustBeNonnegative} = 5e6*ones(6,1) + args.Cr (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) + args.Ka (6,1) double {mustBeNumeric, mustBeNonnegative} = 15e6*ones(6,1) + args.Ca (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1) end-Add Stiffness and Damping properties of each strut
-+-+-stewart.Kdi = args.Kdi; -stewart.Cdi = args.Cdi; +Compute the total stiffness and damping
++-+stewart.Ksi = args.Ksi; -stewart.Csi = args.Csi; +
K = args.Ka + args.Kr; +C = args.Ca + args.Cr; +
++Populate the
+stewartstructure++stewart.actuators.type = 2; -stewart.Ki = args.Ksi + args.Kdi; -stewart.Ci = args.Csi + args.Cdi; +stewart.actuators.Ka = args.Ka; +stewart.actuators.Ca = args.Ca; -stewart.actuator_type = 2; +stewart.actuators.Kr = args.Kr; +stewart.actuators.Cr = args.Cr; + +stewart.actuators.K = K; +stewart.actuators.C = K;
-5.9
-initializeJointDynamics: Add Stiffness and Damping properties for spherical joints++5.10
+initializeJointDynamics: Add Stiffness and Damping properties for spherical joints-@@ -1567,9 +1794,9 @@ This Matlab function is accessible here<
-Function description
-+ --Optional Parameters
-++-Optional Parameters
+arguments stewart - args.Ksbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) - args.Csbi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.Ksti (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) - args.Csti (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) - args.Kubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) - args.Cubi (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) - args.disable logical {mustBeNumericOrLogical} = false + args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'universal' + args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'univesal_p', 'spherical_p'})} = 'spherical' + args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) + args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) + args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) + args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 15*ones(6,1) + args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1) + args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 20*ones(6,1) + args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1) end-Add Stiffness and Damping properties of each strut
-+-++ +Add Actuator Type
++-+if args.disable - stewart.Ksbi = zeros(6,1); - stewart.Csbi = zeros(6,1); - stewart.Ksti = zeros(6,1); - stewart.Csti = zeros(6,1); - stewart.Kubi = zeros(6,1); - stewart.Cubi = zeros(6,1); -else - stewart.Ksbi = args.Ksbi; - stewart.Csbi = args.Csbi; - stewart.Ksti = args.Ksti; - stewart.Csti = args.Csti; - stewart.Kubi = args.Kubi; - stewart.Cubi = args.Cubi; +
switch args.type_F + case 'universal' + stewart.joints_F.type = 1; + case 'spherical' + stewart.joints_F.type = 2; + case 'universal_p' + stewart.joints_F.type = 3; + case 'spherical_p' + stewart.joints_F.type = 4; end + +switch args.type_M + case 'universal' + stewart.joints_M.type = 1; + case 'spherical' + stewart.joints_M.type = 2; + case 'universal_p' + stewart.joints_M.type = 3; + case 'spherical_p' + stewart.joints_M.type = 4; +end +
+++ +Add Stiffness and Damping in Translation of each strut
++++Translation Stiffness +
+++ +stewart.joints_M.Kx = zeros(6,1); +stewart.joints_M.Ky = zeros(6,1); +stewart.joints_M.Kz = zeros(6,1); + +stewart.joints_F.Kx = zeros(6,1); +stewart.joints_F.Ky = zeros(6,1); +stewart.joints_F.Kz = zeros(6,1); +
++Translation Damping +
+++stewart.joints_M.Cx = zeros(6,1); +stewart.joints_M.Cy = zeros(6,1); +stewart.joints_M.Cz = zeros(6,1); + +stewart.joints_F.Cx = zeros(6,1); +stewart.joints_F.Cy = zeros(6,1); +stewart.joints_F.Cz = zeros(6,1); +
++Add Stiffness and Damping in Rotation of each strut
+++Rotational Stiffness +
+++ + +stewart.joints_M.Kf = args.Kf_M; +stewart.joints_M.Kt = args.Kf_M; + +stewart.joints_F.Kf = args.Kf_F; +stewart.joints_F.Kt = args.Kf_F; +
++Rotational Damping +
++stewart.joints_M.Cf = args.Cf_M; +stewart.joints_M.Ct = args.Cf_M; + +stewart.joints_F.Cf = args.Cf_F; +stewart.joints_F.Ct = args.Cf_F;
-5.10
-displayArchitecture: 3D plot of the Stewart platform architecture+++ +5.11
+initializeInertialSensor: Initialize the inertial sensor in each strut+ ++ ++This Matlab function is accessible here. +
+++ +Function description
+++++function [stewart] = initializeInertialSensor(stewart, args) +% initializeInertialSensor - Initialize the inertial sensor in each strut +% +% Syntax: [stewart] = initializeInertialSensor(args) +% +% Inputs: +% - args - Structure with the following fields: +% - type - 'geophone', 'accelerometer', 'none' +% - mass [1x1] - Weight of the inertial mass [kg] +% - freq [1x1] - Cutoff frequency [Hz] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - stewart.sensors.inertial +% - type - 1 (geophone), 2 (accelerometer), 3 (none) +% - K [1x1] - Stiffness [N/m] +% - C [1x1] - Damping [N/(m/s)] +% - M [1x1] - Inertial Mass [kg] +% - G [1x1] - Gain +
+++ +Optional Parameters
+++++arguments + stewart + args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none' + args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2 + args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3 +end ++++ +Compute the properties of the sensor
+++++sensor = struct(); + +switch args.type + case 'geophone' + sensor.type = 1; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + case 'accelerometer' + sensor.type = 2; + + sensor.M = args.mass; + sensor.K = sensor.M * (2*pi*args.freq)^2; + sensor.C = 2*sqrt(sensor.M * sensor.K); + sensor.G = -sensor.K/sensor.M; + case 'none' + sensor.type = 3; +end +
+++Populate the
+stewartstructure++++stewart.sensors.inertial = sensor; +
++5.12
+displayArchitecture: 3D plot of the Stewart platform architecture+-@@ -1655,9 +2053,9 @@ This Matlab function is accessible here.
-Function description
-++Function description
+-function [] = displayArchitecture(stewart, args) % displayArchitecture - 3D plot of the Stewart platform architecture @@ -1685,9 +2083,9 @@ This Matlab function is accessible here.
-Optional Parameters
-++Optional Parameters
+-arguments stewart @@ -1696,10 +2094,10 @@ This Matlab function is accessible here. 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.frames logical {mustBeNumericOrLogical} = true + args.legs logical {mustBeNumericOrLogical} = true + args.joints logical {mustBeNumericOrLogical} = true + args.labels logical {mustBeNumericOrLogical} = true args.platforms logical {mustBeNumericOrLogical} = true end@@ -1707,9 +2105,33 @@ This Matlab function is accessible here.-Figure Creation, Frames and Homogeneous transformations
-+++ + +Check the
+stewartstructure elements++++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; +
++Figure Creation, Frames and Homogeneous transformations
+-The reference frame of the 3d plot corresponds to the frame \(\{F\}\).
@@ -1723,11 +2145,11 @@ 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), stewart.FO_A; ... +
FTa = [eye(3), FO_A; ... zeros(1,3), 1]; ATb = [args.ARB, args.AP; ... zeros(1,3), 1]; -BTm = [eye(3), -stewart.MO_B; ... +BTm = [eye(3), -MO_B; ... zeros(1,3), 1]; FTm = FTa*ATb*BTm; @@ -1738,7 +2160,7 @@ FTm = FTa*ATb*BTm; Let’s define a parameter that define the length of the unit vectors used to display the frames.-@@ -1746,15 +2168,15 @@ Let’s define a parameter that define the length of the unit vectors used t Let’s define a parameter used to position the labels with respect to the center of the element.d_unit_vector = stewart.H/4; +
d_unit_vector = H/4;-d_label = stewart.H/20; +
d_label = H/20;-Fixed Base elements
-++Fixed Base elements
+Let’s first plot the frame \(\{F\}\).
@@ -1777,16 +2199,14 @@ Let’s first plot the frame \(\{F\}\). Now plot the frame \(\{A\}\) fixed to the Base.-Fa = stewart.FO_A; - -if args.frames - quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ... +
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(Fa(1) + d_label, ... - Fa(2) + d_label, ... - Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color); + text(FO_A(1) + d_label, ... + FO_A(2) + d_label, ... + FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color); end end
@@ -1796,11 +2216,11 @@ Now plot the frame \(\{A\}\) fixed to the Base. Let’s then plot the circle corresponding to the shape of the Fixed base.--if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr') +
if args.platforms && stewart.platform_F.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal center = [0; 0; 0]; % Center of the circle - radius = stewart.platforms.Fpr; % Radius of the circle [m] + radius = stewart.platform_F.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); @@ -1816,14 +2236,14 @@ Let’s now plot the position and labels of the Fixed Joints
if args.joints - scatter3(stewart.Fa(1,:), ... - stewart.Fa(2,:), ... - stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color); + scatter3(Fa(1,:), ... + Fa(2,:), ... + Fa(3,:), 'MarkerEdgeColor', args.F_color); if args.labels - for i = 1:size(stewart.Fa,2) - text(stewart.Fa(1,i) + d_label, ... - stewart.Fa(2,i), ... - stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color); + 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 @@ -1832,9 +2252,9 @@ Let’s now plot the position and labels of the Fixed Joints
-Mobile Platform elements
-++Mobile Platform elements
+Plot the frame \(\{M\}\).
@@ -1859,7 +2279,7 @@ Plot the frame \(\{M\}\). Plot the frame \(\{B\}\).-FB = stewart.FO_A + args.AP; +
FB = FO_A + args.AP; if args.frames FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors @@ -1879,11 +2299,11 @@ Plot the frame \(\{B\}\). Let’s then plot the circle corresponding to the shape of the Mobile platform.
--if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr') +
if args.platforms && stewart.platform_M.type == 1 theta = [0:0.01:2*pi+0.01]; % Angles [rad] v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal center = Fm(1:3); % Center of the circle - radius = stewart.platforms.Mpr; % Radius of the circle [m] + radius = stewart.platform_M.R; % Radius of the circle [m] points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); @@ -1899,7 +2319,7 @@ Plot the position and labels of the rotation joints fixed to the mobile platform
if args.joints - Fb = FTm*[stewart.Mb;ones(1,6)]; + Fb = FTm*[Mb;ones(1,6)]; scatter3(Fb(1,:), ... Fb(2,:), ... @@ -1918,23 +2338,23 @@ Plot the position and labels of the rotation joints fixed to the mobile platform
-Legs
-++Legs
+-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([stewart.Fa(1,i), Fb(1,i)], ... - [stewart.Fa(2,i), Fb(2,i)], ... - [stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color); + 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((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ... - (stewart.Fa(2,i)+Fb(2,i))/2, ... - (stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color); + 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 @@ -1943,9 +2363,9 @@ Plot the legs connecting the joints of the fixed base to the joints of the mobil
-5.10.1 Figure parameters
-++5.12.1 Figure parameters
+view([1 -0.6 0.4]); axis equal; @@ -1966,7 +2386,7 @@ Plot the legs connecting the joints of the fixed base to the joints of the mobil
-diff --git a/stewart-architecture.org b/stewart-architecture.org index fb0d8f2..01a1bc3 100644 --- a/stewart-architecture.org +++ b/stewart-architecture.org @@ -206,10 +206,11 @@ By following this procedure, we obtain a Matlab structure =stewart= that contain ** Example of the initialization of a Stewart Platform Let's first define the Stewart Platform Geometry. #+begin_src matlab - stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); + stewart = initializeStewartPlatform(); + stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3); stewart = generateGeneralConfiguration(stewart); stewart = computeJointsPose(stewart); - stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3)); + stewart = initializeStewartPose(stewart, 'AP', [0;0;0], 'ARB', eye(3)); #+end_src Then, define the inertia and geometry of the fixed base, mobile platform and struts. @@ -218,12 +219,18 @@ Then, define the inertia and geometry of the fixed base, mobile platform and str stewart = initializeCylindricalStruts(stewart); #+end_src -Finally, initialize the strut stiffness and damping properties. +We initialize the strut stiffness and damping properties. #+begin_src matlab - stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); + stewart = initializeStrutDynamics(stewart, 'K', 1e6*ones(6,1), 'C', 1e2*ones(6,1)); + stewart = initializeAmplifiedStrutDynamics(stewart); stewart = initializeJointDynamics(stewart); #+end_src +And finally the inertial sensors included in each strut. +#+begin_src matlab + stewart = initializeInertialSensor(stewart, 'type', 'none'); +#+end_src + The obtained =stewart= Matlab structure contains all the information for analysis of the Stewart platform and for simulations using Simscape. The function =displayArchitecture= can be used to display the current Stewart configuration: @@ -279,6 +286,69 @@ Let's now move a little bit the top platform and re-display the configuration: * Functions <Created: 2020-02-10 lun. 18:17
+Created: 2020-02-11 mar. 15:15
- 6.1.
