diff --git a/figs/frame_definition.png b/figs/frame_definition.png new file mode 100644 index 0000000..7b08007 Binary files /dev/null and b/figs/frame_definition.png differ diff --git a/figs/joint_location.png b/figs/joint_location.png new file mode 100644 index 0000000..d065427 Binary files /dev/null and b/figs/joint_location.png differ diff --git a/figs/length_orientation_struts.png b/figs/length_orientation_struts.png new file mode 100644 index 0000000..51f5ebf Binary files /dev/null and b/figs/length_orientation_struts.png differ diff --git a/figs/stewart_platform_actuator.png b/figs/stewart_platform_actuator.png new file mode 100644 index 0000000..a486ea7 Binary files /dev/null and b/figs/stewart_platform_actuator.png differ diff --git a/src/initializeJointDynamics.m b/src/initializeJointDynamics.m new file mode 100644 index 0000000..586a469 --- /dev/null +++ b/src/initializeJointDynamics.m @@ -0,0 +1,23 @@ +function [stewart] = initializeJointDynamics(stewart, args) +% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints +% +% Syntax: [stewart] = initializeJointDynamics(args) +% +% Inputs: +% - args - Structure with the following fields: +% - Kri [6x1] - Rotational Stiffness for each spherical joints [N/rad] +% - Cri [6x1] - Damping of each spherical joint [N/(rad/s)] +% +% Outputs: +% - stewart - updated Stewart structure with the added fields: +% - Kri [6x1] - Rotational Stiffness for each spherical joints [N/rad] +% - Cri [6x1] - Damping of each spherical joint [N/(rad/s)] + +arguments + stewart + args.Kri (6,1) double {mustBeNumeric, mustBePositive} = zeros(6,1) + args.Cri (6,1) double {mustBeNumeric, mustBePositive} = zeros(6,1) +end + +stewart.Kri = args.Kri; +stewart.Cri = args.Cri; diff --git a/stewart-architecture.html b/stewart-architecture.html index 1cb20d3..b033165 100644 --- a/stewart-architecture.html +++ b/stewart-architecture.html @@ -1,11 +1,10 @@ -
- +initializeFramesPositions
: Initialize the positions of frames {A}, {B}, {F} and {M}
+generateCubicConfiguration
: Generate a Cubic Configuration
-
generateGeneralConfiguration
: Generate a Very General Configuration
+initializeFramesPositions
: Initialize the positions of frames {A}, {B}, {F} and {M}
+
computeJointsPose
: Compute the Pose of the Joints
+generateCubicConfiguration
: Generate a Cubic Configuration
initializeStrutDynamics
: Add Stiffness and Damping properties of each strut
+generateGeneralConfiguration
: Generate a Very General Configuration
computeJacobian
: Compute the Jacobian Matrix
+computeJointsPose
: Compute the Pose of the Joints
initializeStewartPose
: Determine the initial stroke in each leg to have the wanted pose
initializeCylindricalPlatforms
: Initialize the geometry of the Fixed and Mobile Platforms
-
initializeCylindricalStruts
: Define the mass and moment of inertia of cylindrical struts
+initializeCylindricalPlatforms
: Initialize the geometry of the Fixed and Mobile Platforms
initializeCylindricalStruts
: Define the inertia of cylindrical struts
+
initializeStewartPose
: Determine the initial stroke in each leg to have the wanted pose
+initializeStrutDynamics
: Add Stiffness and Damping properties of each strut
initializeJointDynamics
: Add Stiffness and Damping properties for spherical joints
--Stewart platforms are generated in multiple steps. +In this document is explained how the Stewart Platform architecture is defined.
-We define 4 important frames: +Some efforts has been made such that the procedure for the definition of the Stewart Platform architecture is as logical and clear as possible. +
+ ++When possible, the notations are compatible with the one used in taghirad13_paral. +
+ ++The definition of the Stewart platform is done in three main parts: +
++In section 4, the procedure the initialize the Stewart platform is summarize and the associated Matlab code is shown. +
+ ++Finally, all the Matlab function used to initialize the Stewart platform are described in section 5. +
+ ++Stewart platforms are generated in multiple steps: +
++This steps are detailed below. +
++We define 4 important frames (see Figure 1):
-Then, we define the location of the spherical joints: +Even though frames \(\{A\}\) and \(\{B\}\) don’t usually correspond to physical elements, they are of primary importance. +Firstly, they are used for the definition of the motion of the Mobile platform with respect to the fixed frame: +
++The frames \(\{A\}\) and \(\{B\}\) are used for all the kinematic analysis (Jacobian, Stiffness matrix, …). +
+ ++Typical choice of \(\{A\}\) and \(\{B\}\) are: +
+
+The definition of the frames is done with the initializeFramesPositions
function (link);
+
+
+Figure 1: Definition of the Frames for the Stewart Platform
++Then, we define the location of the spherical joints (see Figure 2):
-We define the rest position of the Stewart platform: +The location of the joints will define the Geometry of the Stewart platform. +Many characteristics of the platform depend on the location of the joints. +
+ +
+The location of the joints can be set to arbitrary positions (function generateGeneralConfiguration
described here) or can be computed to obtain specific configurations such as:
generateCubicConfiguration
(link).-From \(\bm{a}_{i}\) and \(\bm{b}_{i}\), we can determine the length and orientation of each strut: +The location of the spherical joints are then given by \({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\). +
+ + ++
+Figure 2: Position of the Spherical/Universal joints for the Stewart Platform
++From the location of the joints (\({}^{F}\bm{a}_{i}\) and \({}^{M}\bm{b}_{i}\)), we compute the length \(l_i\) and orientation of each strut \(\hat{\bm{s}}_i\) (unit vector aligned with the strut). +The length and orientation of each strut is represented in figure 3. +
+ +
+This is done with the computeJointsPose
function (link).
+
+
+Figure 3: Length \(l_i\) and orientation \(\hat{\bm{s}}_i\) of the Stewart platform struts
++We may want to initialize the Stewart platform in some position and orientation that corresponds to its rest position. +
+ ++To do so, we choose:
-The position of the Spherical joints can be computed using various methods:
+Then, the function initializeStewartPose
(link) compute the corresponding initial and rest position of each of the strut.
+
+Now that the geometry of the Stewart platform has been defined, we have to choose the inertia of:
-For Simscape, we need: +The inertia of these elements will modify the dynamics of the systems. +It is thus important to set them properly. +
++In order to set the inertia of the fixed and mobile platforms, we can use the following function that assume that both platforms are cylindrical:
initializeCylindricalPlatforms
(link): by choosing the height, radius and mass of the platforms, it computes the inertia matrix that will be used for simulation+Similarly for the struts, we suppose here that they have a cylindrical shape. +They are initialize with the following function: +
+initializeCylindricalStruts
(link): the two parts of each strut are supposed to by cylindrical. We can set the mass and geometry of both strut parts.+The global stiffness and damping of the Stewart platform depends on its geometry but also on the stiffness and damping of: +
++Each Actuator is modeled by 3 elements in parallel (Figure 4): +
++
+Figure 4: Model of the Stewart platform actuator
+
+The initialization of the stiffness and damping properties of the actuators is done with the initializeStrutDynamics
(link).
+
+Even though we often suppose that the spherical joint are perfect in the sense that we neglect its stiffness and damping, we can set some rotation stiffness and damping of each of the spherical/universal joints. +
+ +
+This is done with the initializeJointDynamics
function (link).
+
The procedure to define the Stewart platform is the following:
@@ -483,61 +684,67 @@ We can do that using various methods depending on the wanted architecture:generateCubicConfiguration
permits to generate a cubic configurationcomputeJointsPose
function.computeJointsPose
function.
+If wanted, compute the rest position of each strut to have the wanted pose of the mobile platform with the function initializeStewartPose
.
+initializeCylindricalPlatforms
and initializeCylindricalStruts
By following this procedure, we obtain a Matlab structure stewart
that contains all the information for the Simscape model and for further analysis.
open('stewart_platform.slx')
-
-+Let’s first define the Stewart Platform Geometry. +
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); -% stewart = generateCubicConfiguration(stewart, 'Hc', 60e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3); stewart = generateGeneralConfiguration(stewart); stewart = computeJointsPose(stewart); -stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); -stewart = initializeCylindricalPlatforms(stewart); -stewart = initializeCylindricalStruts(stewart); -stewart = computeJacobian(stewart); stewart = initializeStewartPose(stewart, 'AP', [0;0;0.01], 'ARB', eye(3)); - -[Li, dLi] = inverseKinematics(stewart, 'AP', [0;0;0.00001], 'ARB', eye(3)); -[P, R] = forwardKinematicsApprox(stewart, 'dL', dLi);
+Then, define the inertia and geometry of the fixed base, mobile platform and struts. +
+stewart = initializeCylindricalPlatforms(stewart); +stewart = initializeCylindricalStruts(stewart); ++
+Finally, initialize the strut stiffness and damping properties. +
+stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); +stewart = initializeJointDynamics(stewart, 'Ksi', zeros(6,1), 'Csi', zeros(6,1)); ++
+The obtained stewart
Matlab structure contains all the information for analysis of the Stewart platform and for simulations using Simscape.
+
initializeFramesPositions
: Initialize the positions of frames {A}, {B}, {F} and {M}initializeFramesPositions
: Initialize the positions of frames {A}, {B}, {F} and {M}@@ -545,9 +752,9 @@ This Matlab function is accessible her
function [stewart] = initializeFramesPositions(args) % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M} @@ -570,21 +777,21 @@ This Matlab function is accessible her
-
Figure 1: Definition of the position of the frames
+Figure 5: Definition of the position of the frames
arguments args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3 @@ -595,9 +802,9 @@ This Matlab function is accessible her
stewart = struct();@@ -605,9 +812,9 @@ This Matlab function is accessible her
stewart.H = args.H; % Total Height of the Stewart Platform [m] @@ -622,15 +829,11 @@ stewart.FO_A = stewart.MO_B + stewart.FO_M;
generateCubicConfiguration
: Generate a Cubic ConfigurationgenerateCubicConfiguration
: Generate a Cubic Configuration@@ -638,9 +841,9 @@ This Matlab function is accessible he
function [stewart] = generateCubicConfiguration(stewart, args) % generateCubicConfiguration - Generate a Cubic Configuration @@ -665,21 +868,21 @@ This Matlab function is accessible he
-
Figure 2: Cubic Configuration
+Figure 6: Cubic Configuration
arguments stewart @@ -693,9 +896,9 @@ This Matlab function is accessible he
We define the useful points of the cube with respect to the Cube’s center. \({}^{C}C\) are the 6 vertices of the cubes expressed in a frame {C} which is @@ -720,9 +923,9 @@ CCm = [Cc(:,2), Cc(:
We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)).
@@ -743,11 +946,11 @@ stewart.Mb = CCf + [0; 0; args.FOc -generateGeneralConfiguration
: Generate a Very General ConfigurationgenerateGeneralConfiguration
: Generate a Very General ConfigurationJoints 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. @@ -792,9 +995,9 @@ The radius of the circles can be chosen as well as the angles where the joints a
arguments stewart @@ -810,9 +1013,9 @@ The radius of the circles can be chosen as well as the angles where the joints a
stewart.Fa = zeros(3,6); stewart.Mb = zeros(3,6); @@ -829,13 +1032,12 @@ stewart.Mb = zeros(3,6);
computeJointsPose
: Compute the Pose of the JointscomputeJointsPose
: Compute the Pose of the Joints@@ -843,9 +1045,9 @@ This Matlab function is accessible here.
function [stewart] = computeJointsPose(stewart) % computeJointsPose - @@ -876,21 +1078,21 @@ This Matlab function is accessible here.
-
Figure 3: Position and orientation of the struts
+Figure 7: Position and orientation of the struts
stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]); stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]); @@ -902,9 +1104,9 @@ stewart.Ba = stewart.Aa - repmat( stewart.MO_B
stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As @@ -919,9 +1121,9 @@ stewart.l = vecnorm(stewart.Ab - stewart.Aa)
stewart.FRa = zeros(3,3,6); stewart.MRb = zeros(3,3,6); @@ -939,143 +1141,76 @@ stewart.MRb = zeros(3,3,6);
initializeStrutDynamics
: Add Stiffness and Damping properties of each strutinitializeStewartPose
: Determine the initial stroke in each leg to have the wanted posefunction [stewart] = initializeStrutDynamics(stewart, args) -% initializeStrutDynamics - Add Stiffness and Damping properties of each strut +function [stewart] = initializeStewartPose(stewart, args) +% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose +% It uses the inverse kinematic % -% Syntax: [stewart] = initializeStrutDynamics(args) +% Syntax: [stewart] = initializeStewartPose(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)] +% - stewart - A structure with the following fields +% - Aa [3x6] - The positions ai expressed in {A} +% - Bb [3x6] - The positions bi expressed in {B} +% - args - Can have the following fields: +% - AP [3x1] - The wanted position of {B} with respect to {A} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} % % Outputs: % - stewart - updated Stewart structure with the added fields: -% - Ki [6x1] - Stiffness of each strut [N/m] -% - Ci [6x1] - Damping of each strut [N/(m/s)] +% - dLi[6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
arguments stewart - args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1) - args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1) + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) end
stewart.Ki = args.Ki; -stewart.Ci = args.Ci; +[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB); + +stewart.dLi = dLi;
computeJacobian
: Compute the Jacobian MatrixinitializeCylindricalPlatforms
: Initialize the geometry of the Fixed and Mobile Platforms-This Matlab function is accessible here. -
-function [stewart] = computeJacobian(stewart) -% computeJacobian - -% -% Syntax: [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} -% -% Outputs: -% - stewart - With the 3 added field: -% - J [6x6] - The Jacobian Matrix -% - K [6x6] - The Stiffness Matrix -% - C [6x6] - The Compliance Matrix --
stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)']; --
stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J; --
stewart.C = inv(stewart.K); --
initializeCylindricalPlatforms
: Initialize the geometry of the Fixed and Mobile Platforms
@@ -1083,9 +1218,9 @@ This Matlab function is accessible
-
@@ -1182,9 +1317,9 @@ This Matlab function is accessible h
-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\).
-
-From the geometry of the manipulator, the loop closure for each limb, \(i = 1, 2, \dots, 6\) can be written as
-
-To obtain the length of each actuator and eliminate \(\hat{\bm{s}}_i\), it is sufficient to dot multiply each side by itself:
-
-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.
-
-This Matlab function is accessible here.
-
-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}} \]
-
-The position vector corresponds to the first 3 elements.
-
-The next 3 elements are the orientation of {B} with respect to {A} expressed
-using the screw axis.
-
-We then compute the corresponding rotation matrix.
-
-This Matlab function is accessible here.
-
-This Matlab function is accessible here.
- Created: 2020-01-27 lun. 17:41 Created: 2020-01-28 mar. 17:138.1.1 Function description
-Function description
+function [stewart] = initializeCylindricalPlatforms(stewart, args)
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
@@ -1117,9 +1252,9 @@ This Matlab function is accessible
-
8.1.2 Optional Parameters
-Optional Parameters
+arguments
stewart
@@ -1135,9 +1270,9 @@ This Matlab function is accessible
-
8.1.3 Create the
-platforms
structCreate the
+platforms
structplatforms = struct();
@@ -1159,9 +1294,9 @@ platforms.Mpi = diag([1/12
8.1.4 Save the
-platforms
structSave the
+platforms
structstewart.platforms = platforms;
@@ -1170,11 +1305,11 @@ platforms.Mpi = diag([1/12
8.2
-initializeCylindricalStruts
: Define the mass and moment of inertia of cylindrical struts5.7
+initializeCylindricalStruts
: Define the inertia of cylindrical struts8.2.1 Function description
-Function description
+function [stewart] = initializeCylindricalStruts(stewart, args)
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
@@ -1216,9 +1351,9 @@ This Matlab function is accessible h
8.2.2 Optional Parameters
-Optional Parameters
+arguments
stewart
@@ -1234,9 +1369,9 @@ This Matlab function is accessible h
8.2.3 Create the
-struts
structureCreate the
+struts
structurestruts = struct();
@@ -1268,267 +1403,123 @@ struts.Msi = zeros(3, 3, 6);
9
-initializeStewartPose
: Determine the initial stroke in each leg to have the wanted pose5.8
+
-initializeStrutDynamics
: Add Stiffness and Damping properties of each strut9.1 Function description
-Function description
+function [stewart] = initializeStewartPose(stewart, args)
-% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
-% It uses the inverse kinematic
+
function [stewart] = initializeStrutDynamics(stewart, args)
+% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
%
-% Syntax: [stewart] = initializeStewartPose(stewart, args)
+% Syntax: [stewart] = initializeStrutDynamics(args)
%
% Inputs:
-% - stewart - A structure with the following fields
-% - Aa [3x6] - The positions ai expressed in {A}
-% - Bb [3x6] - The positions bi expressed in {B}
-% - args - Can have the following fields:
-% - AP [3x1] - The wanted position of {B} with respect to {A}
-% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+% - args - Structure with the following fields:
+% - Ki [6x1] - Stiffness of each strut [N/m]
+% - Ci [6x1] - Damping of each strut [N/(m/s)]
%
% 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}
+% - Ki [6x1] - Stiffness of each strut [N/m]
+% - Ci [6x1] - Damping of each strut [N/(m/s)]
9.2 Optional Parameters
-Optional Parameters
+arguments
stewart
- args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
- args.ARB (3,3) double {mustBeNumeric} = eye(3)
+ args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
+ args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e1*ones(6,1)
end
9.3 Use the Inverse Kinematic function
-Add Stiffness and Damping properties of each strut
+[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
-
-stewart.dLi = dLi;
+
stewart.Ki = args.Ki;
+stewart.Ci = args.Ci;
10 Utility Functions
-10.1
-inverseKinematics
: Compute Inverse Kinematics5.9
+
-initializeJointDynamics
: Add Stiffness and Damping properties for spherical joints10.1.1 Function description
-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}
+
function [stewart] = initializeJointDynamics(stewart, args)
+% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
%
-% Syntax: [stewart] = inverseKinematics(stewart)
+% Syntax: [stewart] = initializeJointDynamics(args)
%
% Inputs:
-% - stewart - A structure with the following fields
-% - Aa [3x6] - The positions ai expressed in {A}
-% - Bb [3x6] - The positions bi expressed in {B}
-% - args - Can have the following fields:
-% - AP [3x1] - The wanted position of {B} with respect to {A}
-% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+% - args - Structure with the following fields:
+% - Kri [6x1] - Rotational Stiffness for each spherical joints [N/rad]
+% - Cri [6x1] - Damping of each spherical joint [N/(rad/s)]
%
% 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}
+% - stewart - updated Stewart structure with the added fields:
+% - Kri [6x1] - Rotational Stiffness for each spherical joints [N/rad]
+% - Cri [6x1] - Damping of each spherical joint [N/(rad/s)]
10.1.2 Optional Parameters
-Optional Parameters
+arguments
stewart
- args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
- args.ARB (3,3) double {mustBeNumeric} = eye(3)
+ args.Kri (6,1) double {mustBeNumeric, mustBePositive} = zeros(6,1)
+ args.Cri (6,1) double {mustBeNumeric, mustBePositive} = zeros(6,1)
end
10.1.3 Theory
-10.1.4 Compute
-Add Stiffness and Damping properties of each strut
+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));
-
-dLi = Li-stewart.l;
-
-10.2
-forwardKinematicsApprox
: Compute the Forward Kinematics10.2.1 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
-% the Jacobian Matrix
-%
-% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
-%
-% Inputs:
-% - stewart - A structure with the following fields
-% - J [6x6] - The Jacobian Matrix
-% - args - Can have the following fields:
-% - dL [6x1] - Displacement of each strut [m]
-%
-% Outputs:
-% - P [3x1] - The estimated position of {B} with respect to {A}
-% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
-
-10.2.2 Optional Parameters
-arguments
- stewart
- args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
-end
-
-10.2.3 Computation
-X = stewart.J\args.dL;
-
-P = X(1:3);
-
-theta = norm(X(4:6));
-s = X(4:6)/theta;
-
-R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
- s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
- s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
+
stewart.Kri = args.Kri;
+stewart.Cri = args.Cri;
11 Other Elements
-11.1 Z-Axis Geophone
-Bibliography
+
-
-function [geophone] = initializeZAxisGeophone(args)
- arguments
- args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
- args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
- end
-
- %%
- geophone.m = args.mass;
-
- %% The Stiffness is set to have the damping resonance frequency
- geophone.k = geophone.m * (2*pi*args.freq)^2;
-
- %% We set the damping value to have critical damping
- geophone.c = 2*sqrt(geophone.m * geophone.k);
-
- %% Save
- save('./mat/geophone_z_axis.mat', 'geophone');
-end
-
-11.2 Z-Axis Accelerometer
-function [accelerometer] = initializeZAxisAccelerometer(args)
- arguments
- args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
- args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
- end
-
- %%
- accelerometer.m = args.mass;
-
- %% The Stiffness is set to have the damping resonance frequency
- accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
-
- %% We set the damping value to have critical damping
- accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
-
- %% Gain correction of the accelerometer to have a unity gain until the resonance
- accelerometer.gain = -accelerometer.k/accelerometer.m;
-
- %% Save
- save('./mat/accelerometer_z_axis.mat', 'accelerometer');
-end
-
-