diff --git a/simscape-model.org b/simscape-model.org index d4e5f02..7ad1b68 100644 --- a/simscape-model.org +++ b/simscape-model.org @@ -433,85 +433,6 @@ This Matlab function is accessible [[file:src/initializeStrutDynamics.m][here]]. stewart.Ci = args.Ci; #+end_src -* =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts -:PROPERTIES: -:header-args:matlab+: :tangle src/initializeCylindricalStruts.m -:header-args:matlab+: :comments none :mkdirp yes :eval no -:END: -<> - -This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]]. - -** Function description -#+begin_src matlab - function [stewart] = initializeCylindricalStruts(stewart, args) - % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts - % - % Syntax: [stewart] = initializeCylindricalStruts(args) - % - % Inputs: - % - args - Structure with the following fields: - % - Fsm [1x1] - Mass of the Fixed part of the struts [kg] - % - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] - % - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] - % - Msm [1x1] - Mass of the Mobile part of the struts [kg] - % - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] - % - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] - % - % Outputs: - % - stewart - updated Stewart structure with the added fields: - % - struts [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] -#+end_src - -** Optional Parameters -#+begin_src matlab - arguments - stewart - args.Fsm (6,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Fsh (6,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Fsr (6,1) double {mustBeNumeric, mustBePositive} = 5e-3 - args.Msm (6,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Msh (6,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Msr (6,1) double {mustBeNumeric, mustBePositive} = 5e-3 - end -#+end_src - -** Add Stiffness and Damping properties of each strut -#+begin_src matlab - struts = struct(); - - struts.Fsm = ones(6,1).*args.Fsm; - struts.Msm = ones(6,1).*args.Msm; - - 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.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]); - end -#+end_src - -#+begin_src matlab - stewart.struts = struts; -#+end_src - * =computeJacobian=: Compute the Jacobian Matrix :PROPERTIES: :header-args:matlab+: :tangle src/computeJacobian.m @@ -555,6 +476,161 @@ This Matlab function is accessible [[file:src/computeJacobian.m][here]]. stewart.C = inv(stewart.K); #+end_src +* Initialize the Geometry of the Mechanical Elements +** =initializeCylindricalStruts=: Define the mass and moment of inertia of cylindrical struts +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeCylindricalStruts.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeCylindricalStruts.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalStruts(stewart, args) + % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts + % + % Syntax: [stewart] = initializeCylindricalStruts(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fsm [1x1] - Mass of the Fixed part of the struts [kg] + % - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m] + % - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m] + % - Msm [1x1] - Mass of the Mobile part of the struts [kg] + % - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m] + % - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - struts [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] +#+end_src + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + end +#+end_src + +*** Create the =struts= structure +#+begin_src matlab + struts = struct(); + + struts.Fsm = ones(6,1).*args.Fsm; + struts.Msm = ones(6,1).*args.Msm; + + 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.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]); + end +#+end_src + +#+begin_src matlab + stewart.struts = struts; +#+end_src + +** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms +:PROPERTIES: +:header-args:matlab+: :tangle src/initializeCylindricalPlatforms.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/initializeCylindricalPlatforms.m][here]]. + +*** Function description +#+begin_src matlab + function [stewart] = initializeCylindricalPlatforms(stewart, args) + % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms + % + % Syntax: [stewart] = initializeCylindricalPlatforms(args) + % + % Inputs: + % - args - Structure with the following fields: + % - Fpm [1x1] - Fixed Platform Mass [kg] + % - Fph [1x1] - Fixed Platform Height [m] + % - Fpr [1x1] - Fixed Platform Radius [m] + % - Mpm [1x1] - Mobile Platform Mass [kg] + % - Mph [1x1] - Mobile Platform Height [m] + % - Mpr [1x1] - Mobile Platform Radius [m] + % + % Outputs: + % - stewart - updated Stewart structure with the added fields: + % - 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] +#+end_src + +*** Optional Parameters +#+begin_src matlab + arguments + stewart + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 + end +#+end_src + +*** Create the =platforms= struct +#+begin_src matlab + platforms = struct(); + + 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]); +#+end_src + +*** Save the =platforms= struct +#+begin_src matlab + stewart.platforms = platforms; +#+end_src + * Utility Functions ** =inverseKinematics=: Compute Inverse Kinematics :PROPERTIES: diff --git a/simscape_subsystems/Fixed_Based.slx b/simscape_subsystems/Fixed_Based.slx new file mode 100644 index 0000000..1acc50c Binary files /dev/null and b/simscape_subsystems/Fixed_Based.slx differ diff --git a/simscape_subsystems/Mobile_Platform.slx b/simscape_subsystems/Mobile_Platform.slx new file mode 100644 index 0000000..9b7fce0 Binary files /dev/null and b/simscape_subsystems/Mobile_Platform.slx differ diff --git a/simscape_subsystems/Stewart_Platform.slx b/simscape_subsystems/Stewart_Platform.slx new file mode 100644 index 0000000..f02bde4 Binary files /dev/null and b/simscape_subsystems/Stewart_Platform.slx differ diff --git a/simulink/stewart_active_damping.slx b/simulink/stewart_active_damping.slx index 0093beb..43c621d 100644 Binary files a/simulink/stewart_active_damping.slx and b/simulink/stewart_active_damping.slx differ diff --git a/simulink/stewart_platform.slx b/simulink/stewart_platform.slx deleted file mode 100644 index 074cdc3..0000000 Binary files a/simulink/stewart_platform.slx and /dev/null differ diff --git a/simulink/stewart_platform_control.slx b/simulink/stewart_platform_control.slx index f166d91..aa38e58 100644 Binary files a/simulink/stewart_platform_control.slx and b/simulink/stewart_platform_control.slx differ diff --git a/simulink/stewart_platform_dynamics.slx b/simulink/stewart_platform_dynamics.slx index 59d9402..b072dde 100644 Binary files a/simulink/stewart_platform_dynamics.slx and b/simulink/stewart_platform_dynamics.slx differ diff --git a/simulink/stewart_platform_tests.slx b/simulink/stewart_platform_tests.slx new file mode 100644 index 0000000..8c5e7a7 Binary files /dev/null and b/simulink/stewart_platform_tests.slx differ diff --git a/simulink/stewart_strut.slx b/simulink/stewart_strut.slx index 61992fd..263700d 100644 Binary files a/simulink/stewart_strut.slx and b/simulink/stewart_strut.slx differ diff --git a/src/initializeCylindricalPlatforms.m b/src/initializeCylindricalPlatforms.m new file mode 100644 index 0000000..6f93b1f --- /dev/null +++ b/src/initializeCylindricalPlatforms.m @@ -0,0 +1,53 @@ +function [stewart] = initializeCylindricalPlatforms(stewart, args) +% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms +% +% Syntax: [stewart] = initializeCylindricalPlatforms(args) +% +% Inputs: +% - args - Structure with the following fields: +% - Fpm [1x1] - Fixed Platform Mass [kg] +% - Fph [1x1] - Fixed Platform Height [m] +% - Fpr [1x1] - Fixed Platform Radius [m] +% - Mpm [1x1] - Mobile Platform Mass [kg] +% - Mph [1x1] - Mobile Platform Height [m] +% - Mpr [1x1] - Mobile Platform Radius [m] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - 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] + +arguments + stewart + args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3 + args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1 + args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 + args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3 +end + +platforms = struct(); + +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]); + +stewart.platforms = platforms; diff --git a/src/initializeCylindricalStruts.m b/src/initializeCylindricalStruts.m index 9f6d188..a83d0c5 100644 --- a/src/initializeCylindricalStruts.m +++ b/src/initializeCylindricalStruts.m @@ -26,12 +26,12 @@ function [stewart] = initializeCylindricalStruts(stewart, args) arguments stewart - args.Fsm (6,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Fsh (6,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Fsr (6,1) double {mustBeNumeric, mustBePositive} = 5e-3 - args.Msm (6,1) double {mustBeNumeric, mustBePositive} = 0.1 - args.Msh (6,1) double {mustBeNumeric, mustBePositive} = 50e-3 - args.Msr (6,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 + args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1 + args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3 + args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 end struts = struct();