tdehaeze/stewart-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add few functions and function documentation

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-01-06 18:17:22 +01:00
parent 9d88faee5a
commit 0f635af60f
10 changed files with 765 additions and 201 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
src/computeJacobian.m Normal file
View File

@@ -0,0 +1,21 @@
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);

30
src/forwardKinematics.m Normal file
View File

@@ -0,0 +1,30 @@
function [P, R] = forwardKinematics(stewart, args)
% forwardKinematics - Computed the pose of {B} with respect to {A} from the length of each strut
%
% Syntax: [in_data] = forwardKinematics(stewart)
%
% Inputs:
% - stewart - A structure with the following fields
% - J [6x6] - The Jacobian Matrix
% - args - Can have the following fields:
% - L [6x1] - Length of each strut [m]
%
% Outputs:
% - P [3x1] - The estimated position of {B} with respect to {A}
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
arguments
stewart
args.L (6,1) double {mustBeNumeric} = zeros(6,1)
end
X = stewart.J\args.L;
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)];

4
src/generateCubicConfiguration.m
View File

@@ -8,7 +8,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
% - 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 cute with respect to {F} [m]
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
%
@@ -20,7 +20,7 @@ function [stewart] = generateCubicConfiguration(stewart, args)
arguments
stewart
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
args.FOc (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.FOc (1,1) double {mustBeNumeric} = 50e-3
args.FHa (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
args.MHb (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
end

4
src/initializeFramesPositions.m
View File

@@ -5,7 +5,7 @@ function [stewart] = initializeFramesPositions(args)
%
% Inputs:
% - args - Can have the following fields:
% - H [1x1] - Total Height of the Stewart Platform [m]
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
%
% Outputs:
@@ -17,7 +17,7 @@ function [stewart] = initializeFramesPositions(args)
arguments
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
args.MO_B (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
end
stewart = struct();

26
src/inverseKinematics.m Normal file
View File

@@ -0,0 +1,26 @@
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}
arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
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));
dLi = Li-stewart.l;