diff --git a/matlab/stewart_platform_model.slx b/matlab/stewart_platform_model.slx index f09ab49..55e11fb 100644 Binary files a/matlab/stewart_platform_model.slx and b/matlab/stewart_platform_model.slx differ diff --git a/org/kinematic-study.org b/org/kinematic-study.org index 41b861c..7f86e64 100644 --- a/org/kinematic-study.org +++ b/org/kinematic-study.org @@ -613,6 +613,64 @@ data2orgtable([1e6*L_min, 1e6*L_max, 1e6*(min(min(rs)))], {}, {'=L_min= [$\mu m$ *** TODO Do that by slice :noexport: using this function https://fr.mathworks.com/help/matlab/ref/contour3.html +* Estimation of the Joint required Stroke +** Introduction :ignore: + +** Matlab Init :noexport:ignore: +#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) + <> +#+end_src + +#+begin_src matlab :exports none :results silent :noweb yes + <> +#+end_src + +#+begin_src matlab :results none :exports none + simulinkproject('../'); +#+end_src + +** Example of the initialization of a Stewart Platform +Let's first define the Stewart Platform Geometry. +#+begin_src matlab + stewart = initializeStewartPlatform(); + stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 150e-3); + stewart = generateGeneralConfiguration(stewart); + stewart = computeJointsPose(stewart); + + As_init = stewart.geometry.As; +#+end_src + +#+begin_src matlab + Tx_max = 50e-6; % Translation [m] + Ty_max = 50e-6; % Translation [m] + Tz_max = 50e-6; % Translation [m] +#+end_src + +#+begin_src matlab + Ps = [2*(dec2bin(0:3^2-2,3)-'0')-1].*[Tx_max Ty_max Tz_max]; +#+end_src + +#+begin_src matlab + flex_ang = zeros(size(Ps, 1), 6); + + for Ps_i = 1:size(Ps, 1) + stewart.geometry.FO_M = [0; 0; 90e-3] + Ps(Ps_i, :)'; + + stewart = generateGeneralConfiguration(stewart); + stewart = computeJointsPose(stewart); + + flex_ang(Ps_i, :) = acos(sum(As_init.*stewart.geometry.As)); + end +#+end_src + +And the maximum bending of the flexible joints is: (in [mrad]) +#+begin_src matlab :results replace value + 1e3*max(max(abs(flex_ang))) +#+end_src + +#+RESULTS: +: 0.90937 + * Functions <> ** =computeJacobian=: Compute the Jacobian Matrix diff --git a/org/stewart-architecture.org b/org/stewart-architecture.org index cbda9e8..b5291fc 100644 --- a/org/stewart-architecture.org +++ b/org/stewart-architecture.org @@ -951,6 +951,8 @@ This Matlab function is accessible [[file:../src/initializeCylindricalStruts.m][ #+begin_src matlab arguments stewart + args.type_F char {mustBeMember(args.type_F,{'cylindrical', 'none'})} = 'cylindrical' + args.type_M char {mustBeMember(args.type_M,{'cylindrical', 'none'})} = 'cylindrical' args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 @@ -995,7 +997,12 @@ This Matlab function is accessible [[file:../src/initializeCylindricalStruts.m][ :UNNUMBERED: t :END: #+begin_src matlab - stewart.struts_M.type = 1; + switch args.type_M + case 'cylindrical' + stewart.struts_M.type = 1; + case 'none' + stewart.struts_M.type = 2; + end stewart.struts_M.I = I_M; stewart.struts_M.M = Msm; @@ -1004,7 +1011,12 @@ This Matlab function is accessible [[file:../src/initializeCylindricalStruts.m][ #+end_src #+begin_src matlab - stewart.struts_F.type = 1; + switch args.type_F + case 'cylindrical' + stewart.struts_F.type = 1; + case 'none' + stewart.struts_F.type = 2; + end stewart.struts_F.I = I_F; stewart.struts_F.M = Fsm; @@ -1237,10 +1249,83 @@ A simplistic model of such amplified actuator is shown in Figure [[fig:amplified stewart.actuators.Cr = args.Cr; stewart.actuators.K = K; - stewart.actuators.C = K; + stewart.actuators.C = C; #+end_src -** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints +** TODO =initializeFlexibleStrutDynamics=: Model each strut with a flexible element +:PROPERTIES: +:header-args:matlab+: :tangle ../src/initializeFlexibleStrutDynamics.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:../src/initializeFlexibleStrutDynamics.m][here]]. + +*** Function description +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + function [stewart] = initializeFlexibleStrutDynamics(stewart, args) + % initializeFlexibleStrutDynamics - Add Stiffness and Damping properties of each strut + % + % Syntax: [stewart] = initializeFlexibleStrutDynamics(args) + % + % Inputs: + % - args - Structure with the following fields: + % - K [nxn] - Vertical stiffness contribution of the piezoelectric stack [N/m] + % - M [nxn] - Vertical damping contribution of the piezoelectric stack [N/(m/s)] + % - xi [1x1] - Vertical (residual) stiffness when the piezoelectric stack is removed [N/m] + % - step_file [6x1] - Vertical (residual) damping when the piezoelectric stack is removed [N/(m/s)] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: +#+end_src + +*** Optional Parameters +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + arguments + stewart + args.K double {mustBeNumeric} = zeros(6,6) + args.M double {mustBeNumeric} = zeros(6,6) + args.H double {mustBeNumeric} = 0 + args.n_xyz double {mustBeNumeric} = zeros(2,3) + args.xi double {mustBeNumeric} = 0.1 + args.step_file char {} = '' + end +#+end_src + +*** Compute the axial offset +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab + stewart.actuators.ax_off = (stewart.geometry.l(1) - args.H)/2; % Axial Offset at the ends of the actuator +#+end_src + +*** Populate the =stewart= structure +:PROPERTIES: +:UNNUMBERED: t +:END: +#+begin_src matlab + stewart.actuators.type = 3; + + stewart.actuators.Km = args.K; + stewart.actuators.Mm = args.M; + + stewart.actuators.n_xyz = args.n_xyz; + stewart.actuators.xi = args.xi; + + stewart.actuators.step_file = args.step_file; + + stewart.actuators.K = args.K(3,3); % Axial Stiffness +#+end_src + +** TODO =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeJointDynamics.m :header-args:matlab+: :comments none :mkdirp yes :eval no @@ -1291,8 +1376,8 @@ This Matlab function is accessible [[file:../src/initializeJointDynamics.m][here #+begin_src matlab arguments stewart - args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'universal' - args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'spherical' + args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'flexible'})} = 'universal' + args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'flexible'})} = '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) @@ -1301,6 +1386,16 @@ This Matlab function is accessible [[file:../src/initializeJointDynamics.m][here 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) + args.K_M double {mustBeNumeric} = zeros(6,6) + args.M_M double {mustBeNumeric} = zeros(6,6) + args.n_xyz_M double {mustBeNumeric} = zeros(2,3) + args.xi_M double {mustBeNumeric} = 0.1 + args.step_file_M char {} = '' + args.K_F double {mustBeNumeric} = zeros(6,6) + args.M_F double {mustBeNumeric} = zeros(6,6) + args.n_xyz_F double {mustBeNumeric} = zeros(2,3) + args.xi_F double {mustBeNumeric} = 0.1 + args.step_file_F char {} = '' end #+end_src @@ -1318,6 +1413,8 @@ This Matlab function is accessible [[file:../src/initializeJointDynamics.m][here stewart.joints_F.type = 3; case 'spherical_p' stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; end switch args.type_M @@ -1329,6 +1426,8 @@ This Matlab function is accessible [[file:../src/initializeJointDynamics.m][here stewart.joints_M.type = 3; case 'spherical_p' stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; end #+end_src @@ -1371,7 +1470,6 @@ Rotational Stiffness stewart.joints_F.Kt = args.Kf_F; #+end_src - Rotational Damping #+begin_src matlab stewart.joints_M.Cf = args.Cf_M; @@ -1381,6 +1479,26 @@ Rotational Damping stewart.joints_F.Ct = args.Cf_F; #+end_src +*** Stiffness and Mass matrices for flexible joint +:PROPERTIES: +:UNNUMBERED: t +:END: + +#+begin_src matlab + stewart.joints_F.M = args.M_F; + stewart.joints_F.K = args.K_F; + stewart.joints_F.n_xyz = args.n_xyz_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.xi = args.xi_F; + stewart.joints_F.step_file = args.step_file_F; + + stewart.joints_M.M = args.M_M; + stewart.joints_M.K = args.K_M; + stewart.joints_M.n_xyz = args.n_xyz_M; + stewart.joints_M.xi = args.xi_M; + stewart.joints_M.step_file = args.step_file_M; +#+end_src + ** =initializeInertialSensor=: Initialize the inertial sensor in each strut :PROPERTIES: :header-args:matlab+: :tangle ../src/initializeInertialSensor.m diff --git a/simscape_subsystems/Stewart_Platform.slx b/simscape_subsystems/Stewart_Platform.slx index af23f5f..0c57e23 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 ffe49ee..78030d3 100644 Binary files a/simscape_subsystems/stewart_strut.slx and b/simscape_subsystems/stewart_strut.slx differ diff --git a/src/initializeAmplifiedStrutDynamics.m b/src/initializeAmplifiedStrutDynamics.m index a22bdb0..950bd6d 100644 --- a/src/initializeAmplifiedStrutDynamics.m +++ b/src/initializeAmplifiedStrutDynamics.m @@ -40,4 +40,4 @@ stewart.actuators.Kr = args.Kr; stewart.actuators.Cr = args.Cr; stewart.actuators.K = K; -stewart.actuators.C = K; +stewart.actuators.C = C; diff --git a/src/initializeCylindricalStruts.m b/src/initializeCylindricalStruts.m index 2bb7257..78ef5d4 100644 --- a/src/initializeCylindricalStruts.m +++ b/src/initializeCylindricalStruts.m @@ -27,6 +27,8 @@ function [stewart] = initializeCylindricalStruts(stewart, args) arguments stewart + args.type_F char {mustBeMember(args.type_F,{'cylindrical', 'none'})} = 'cylindrical' + args.type_M char {mustBeMember(args.type_M,{'cylindrical', 'none'})} = 'cylindrical' args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 @@ -56,14 +58,24 @@ for i = 1:6 1/2 * Msm(i) * Msr(i)^2]); end -stewart.struts_M.type = 1; +switch args.type_M + case 'cylindrical' + stewart.struts_M.type = 1; + case 'none' + stewart.struts_M.type = 2; +end 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; +switch args.type_F + case 'cylindrical' + stewart.struts_F.type = 1; + case 'none' + stewart.struts_F.type = 2; +end stewart.struts_F.I = I_F; stewart.struts_F.M = Fsm; diff --git a/src/initializeJointDynamics.m b/src/initializeJointDynamics.m index e82d417..d974981 100644 --- a/src/initializeJointDynamics.m +++ b/src/initializeJointDynamics.m @@ -29,8 +29,8 @@ function [stewart] = initializeJointDynamics(stewart, args) arguments stewart - args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'universal' - args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p'})} = 'spherical' + args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'flexible'})} = 'universal' + args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'flexible'})} = '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) @@ -39,6 +39,16 @@ arguments 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) + args.K_M double {mustBeNumeric} = zeros(6,6) + args.M_M double {mustBeNumeric} = zeros(6,6) + args.n_xyz_M double {mustBeNumeric} = zeros(2,3) + args.xi_M double {mustBeNumeric} = 0.1 + args.step_file_M char {} = '' + args.K_F double {mustBeNumeric} = zeros(6,6) + args.M_F double {mustBeNumeric} = zeros(6,6) + args.n_xyz_F double {mustBeNumeric} = zeros(2,3) + args.xi_F double {mustBeNumeric} = 0.1 + args.step_file_F char {} = '' end switch args.type_F @@ -50,6 +60,8 @@ switch args.type_F stewart.joints_F.type = 3; case 'spherical_p' stewart.joints_F.type = 4; + case 'flexible' + stewart.joints_F.type = 5; end switch args.type_M @@ -61,6 +73,8 @@ switch args.type_M stewart.joints_M.type = 3; case 'spherical_p' stewart.joints_M.type = 4; + case 'flexible' + stewart.joints_M.type = 5; end stewart.joints_M.Kx = zeros(6,1); @@ -90,3 +104,16 @@ stewart.joints_M.Ct = args.Cf_M; stewart.joints_F.Cf = args.Cf_F; stewart.joints_F.Ct = args.Cf_F; + +stewart.joints_F.M = args.M_F; +stewart.joints_F.K = args.K_F; +stewart.joints_F.n_xyz = args.n_xyz_F; +stewart.joints_F.xi = args.xi_F; +stewart.joints_F.xi = args.xi_F; +stewart.joints_F.step_file = args.step_file_F; + +stewart.joints_M.M = args.M_M; +stewart.joints_M.K = args.K_M; +stewart.joints_M.n_xyz = args.n_xyz_M; +stewart.joints_M.xi = args.xi_M; +stewart.joints_M.step_file = args.step_file_M;