From 7cfd4fef39098417a366d74d1bd1a25d9c210b3c Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Wed, 22 Jan 2020 11:35:38 +0100 Subject: [PATCH] Add a general way to initialize the Stewart config --- simscape-model.html | 412 ++++++++++++++++++----------- simscape-model.org | 63 +++++ src/generateGeneralConfiguration.m | 38 +++ 3 files changed, 356 insertions(+), 157 deletions(-) create mode 100644 src/generateGeneralConfiguration.m diff --git a/simscape-model.html b/simscape-model.html index d6ed546..6bd34d8 100644 --- a/simscape-model.html +++ b/simscape-model.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Stewart Platform - Simscape Model @@ -205,7 +205,7 @@ @licstart The following is the entire license notice for the JavaScript code in this tag. -Copyright (C) 2012-2019 Free Software Foundation, Inc. +Copyright (C) 2012-2020 Free Software Foundation, Inc. The JavaScript code in this tag is free software: you can redistribute it and/or modify it under the terms of the GNU @@ -283,68 +283,76 @@ for the JavaScript code in this tag.

Table of Contents

@@ -417,8 +425,9 @@ For Simscape, we need:
  • The position of the frame \(\{B\}\) with respect to the frame \(\{M\}\): \({}^{M}\bm{O}_{B}\)
    • -
    -

    1 Procedure

    + +
    +

    1 Procedure

    The procedure to define the Stewart platform is the following: @@ -447,12 +456,12 @@ By following this procedure, we obtain a Matlab structure stewart t

    -
    -

    2 Matlab Code

    +
    +

    2 Matlab Code

    -
    -

    2.1 Simscape Model

    +
    +

    2.1 Simscape Model

    open('stewart_platform.slx')
@@ -461,8 +470,8 @@ By following this procedure, we obtain a Matlab structure stewart t
    -
    -

    2.2 Test the functions

    +
    +

    2.2 Test the functions

    stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
@@ -478,11 +487,11 @@ stewart = computeJacobian(stewart);
    -
    -

    3 initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}

    +
    +

    3 initializeFramesPositions: Initialize the positions of frames {A}, {B}, {F} and {M}

    - +

    @@ -490,8 +499,8 @@ This Matlab function is accessible her

    -
    -

    3.1 Function description

    +
    +

    3.1 Function description

    function [stewart] = initializeFramesPositions(args)
@@ -515,11 +524,11 @@ This Matlab function is accessible her
    -
    -

    3.2 Documentation

    +
    +

    3.2 Documentation

    - -
    -

    3.3 Optional Parameters

    +
    +

    3.3 Optional Parameters

    arguments
@@ -540,8 +549,8 @@ This Matlab function is accessible her
    -
    -

    3.4 Initialize the Stewart structure

    +
    +

    3.4 Initialize the Stewart structure

    stewart = struct();
@@ -550,8 +559,8 @@ This Matlab function is accessible her
    -
    -

    3.5 Compute the position of each frame

    +
    +

    3.5 Compute the position of each frame

    stewart.H = args.H; % Total Height of the Stewart Platform [m]
@@ -567,11 +576,11 @@ stewart.FO_A = stewart.MO_B + stewart.FO_M;
    -
    -

    4 generateCubicConfiguration: Generate a Cubic Configuration

    +
    +

    4 generateCubicConfiguration: Generate a Cubic Configuration

    - +

    @@ -579,8 +588,8 @@ This Matlab function is accessible he

    -
    -

    4.1 Function description

    +
    +

    4.1 Function description

    function [stewart] = generateCubicConfiguration(stewart, args)
@@ -606,11 +615,11 @@ This Matlab function is accessible he
    -
    -

    4.2 Documentation

    +
    +

    4.2 Documentation

    - -
    -

    4.3 Optional Parameters

    +
    +

    4.3 Optional Parameters

    arguments
@@ -634,8 +643,8 @@ This Matlab function is accessible he
    -
    -

    4.4 Position of the Cube

    +
    +

    4.4 Position of the Cube

    We define the useful points of the cube with respect to the Cube’s center. @@ -661,8 +670,8 @@ CCm = [Cc(:,2), Cc(:

    -
    -

    4.5 Compute the pose

    +
    +

    4.5 Compute the pose

    We can compute the vector of each leg \({}^{C}\hat{\bm{s}}_{i}\) (unit vector from \({}^{C}C_{f}\) to \({}^{C}C_{m}\)). @@ -684,11 +693,100 @@ stewart.Mb = CCf + [0; 0; args.FOc -

    5 computeJointsPose: Compute the Pose of the Joints

    +
    +

    5 generateGeneralConfiguration: Generate a Very General Configuration

    - + +

    + +

    +This Matlab function is accessible here. +

    +
    + +
    +

    5.1 Function description

    +
    +
    +
    function [stewart] = generateGeneralConfiguration(stewart, args)
+% generateGeneralConfiguration - Generate a Very General Configuration
+%
+% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
+%
+% Inputs:
+%    - stewart - A structure with the following fields
+%        - H   [1x1] - Total height of the platform [m]
+%    - args - Can have the following fields:
+%        - FH  [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
+%        - FR  [1x1] - Radius of the position of the fixed joints in the X-Y [m]
+%        - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
+%        - MH  [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
+%        - FR  [1x1] - Radius of the position of the mobile joints in the X-Y [m]
+%        - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
+%
+% 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}
+
    +
    +
    +
    + +
    +

    5.2 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. +

    +
    +
    + +
    +

    5.3 Optional Parameters

    +
    +
    +
    arguments
+    stewart
+    args.FH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+    args.FR  (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
+    args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
+    args.MH  (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
+    args.MR  (1,1) double {mustBeNumeric, mustBePositive} = 70e-3;
+    args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
+end
+
    +
    +
    +
    + +
    +

    5.4 Compute the pose

    +
    +
    +
    stewart.Fa = zeros(3,6);
+stewart.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];
+end
+
    +
    +
    +
    +
    + +
    +

    6 computeJointsPose: Compute the Pose of the Joints

    +
    +

    +

    @@ -696,9 +794,9 @@ This Matlab function is accessible here.

    -
    -

    5.1 Function description

    -
    +
    +

    6.1 Function description

    +
    function [stewart] = computeJointsPose(stewart)
 % computeJointsPose -
@@ -729,11 +827,11 @@ This Matlab function is accessible here.
    -
    -

    5.2 Documentation

    -
    +
    +

    6.2 Documentation

    +
    -
    +

    stewart-struts.png

    Figure 3: Position and orientation of the struts

    @@ -741,9 +839,9 @@ This Matlab function is accessible here.
    -
    -

    5.3 Compute the position of the Joints

    -
    +
    +

    6.3 Compute the position of the Joints

    +
    stewart.Aa = stewart.Fa - repmat(stewart.FO_A, [1, 6]);
 stewart.Bb = stewart.Mb - repmat(stewart.MO_B, [1, 6]);
@@ -755,9 +853,9 @@ stewart.Ba = stewart.Aa - repmat( stewart.MO_B
    -
    -

    5.4 Compute the strut length and orientation

    -
    +
    +

    6.4 Compute the strut length and orientation

    +
    stewart.As = (stewart.Ab - stewart.Aa)./vecnorm(stewart.Ab - stewart.Aa); % As_i is the i'th vector of As
 
@@ -772,9 +870,9 @@ stewart.l = vecnorm(stewart.Ab - stewart.Aa)
    -
    -

    5.5 Compute the orientation of the Joints

    -
    +
    +

    6.5 Compute the orientation of the Joints

    +
    stewart.FRa = zeros(3,3,6);
 stewart.MRb = zeros(3,3,6);
@@ -792,11 +890,11 @@ stewart.MRb = zeros(3,3,6);
    -
    -

    6 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

    -
    +
    +

    7 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

    +

    - +

    @@ -804,9 +902,9 @@ This Matlab function is accessible here<

    -
    -

    6.1 Function description

    -
    +
    +

    7.1 Function description

    +
    function [stewart] = initializeStrutDynamics(stewart, args)
 % initializeStrutDynamics - Add Stiffness and Damping properties of each strut
@@ -827,23 +925,23 @@ This Matlab function is accessible here<
    -
    -

    6.2 Optional Parameters

    -
    +
    +

    7.2 Optional Parameters

    +
    arguments
     stewart
     args.Ki (6,1) double {mustBeNumeric, mustBePositive} = 1e6*ones(6,1)
-    args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e2*ones(6,1)
+    args.Ci (6,1) double {mustBeNumeric, mustBePositive} = 1e3*ones(6,1)
 end
    -
    -

    6.3 Add Stiffness and Damping properties of each strut

    -
    +
    +

    7.3 Add Stiffness and Damping properties of each strut

    +
    stewart.Ki = args.Ki;
 stewart.Ci = args.Ci;
@@ -853,11 +951,11 @@ stewart.Ci = args.Ci;
    -
    -

    7 computeJacobian: Compute the Jacobian Matrix

    -
    +
    +

    8 computeJacobian: Compute the Jacobian Matrix

    +

    - +

    @@ -865,9 +963,9 @@ This Matlab function is accessible here.

    -
    -

    7.1 Function description

    -
    +
    +

    8.1 Function description

    +
    function [stewart] = computeJacobian(stewart)
 % computeJacobian -
@@ -889,9 +987,9 @@ This Matlab function is accessible here.
    -
    -

    7.2 Compute Jacobian Matrix

    -
    +
    +

    8.2 Compute Jacobian Matrix

    +
    stewart.J = [stewart.As' , cross(stewart.Ab, stewart.As)'];
    @@ -899,9 +997,9 @@ This Matlab function is accessible here.
    -
    -

    7.3 Compute Stiffness Matrix

    -
    +
    +

    8.3 Compute Stiffness Matrix

    +
    stewart.K = stewart.J'*diag(stewart.Ki)*stewart.J;
    @@ -909,9 +1007,9 @@ This Matlab function is accessible here.
    -
    -

    7.4 Compute Compliance Matrix

    -
    +
    +

    8.4 Compute Compliance Matrix

    +
    stewart.C = inv(stewart.K);
    @@ -920,11 +1018,11 @@ This Matlab function is accessible here.
    -
    -

    8 inverseKinematics: Compute Inverse Kinematics

    -
    +
    +

    9 inverseKinematics: Compute Inverse Kinematics

    +

    - +

    @@ -932,9 +1030,9 @@ This Matlab function is accessible here.

    -
    -

    8.1 Function description

    -
    +
    +

    9.1 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}
@@ -957,9 +1055,9 @@ This Matlab function is accessible here.
    -
    -

    8.2 Optional Parameters

    -
    +
    +

    9.2 Optional Parameters

    +
    arguments
     stewart
@@ -971,9 +1069,9 @@ This Matlab function is accessible here.
    -
    -

    8.3 Theory

    -
    +
    +

    9.3 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\).

    @@ -1007,9 +1105,9 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then the
    -
    -

    8.4 Compute

    -
    +
    +

    9.4 Compute

    +
    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));
    @@ -1023,11 +1121,11 @@ Otherwise, when the limbs’ lengths derived yield complex numbers, then the
    -
    -

    9 forwardKinematicsApprox: Compute the Forward Kinematics

    -
    +
    +

    10 forwardKinematicsApprox: Compute the Forward Kinematics

    +

    - +

    @@ -1035,9 +1133,9 @@ This Matlab function is accessible here<

    -
    -

    9.1 Function description

    -
    +
    +

    10.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
@@ -1059,9 +1157,9 @@ This Matlab function is accessible here<
    -
    -

    9.2 Optional Parameters

    -
    +
    +

    10.2 Optional Parameters

    +
    arguments
     stewart
@@ -1072,9 +1170,9 @@ This Matlab function is accessible here<
    -
    -

    9.3 Computation

    -
    +
    +

    10.3 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: @@ -1118,7 +1216,7 @@ We then compute the corresponding rotation matrix.

    Author: Dehaeze Thomas

    -

    Created: 2020-01-06 lun. 18:16

    +

    Created: 2020-01-22 mer. 11:35

    diff --git a/simscape-model.org b/simscape-model.org index 1d64079..a51f428 100644 --- a/simscape-model.org +++ b/simscape-model.org @@ -248,6 +248,69 @@ We now which to compute the position of the joints $a_{i}$ and $b_{i}$. stewart.Mb = CCf + [0; 0; args.FOc-stewart.H] + ((stewart.H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi; #+end_src +* =generateGeneralConfiguration=: Generate a Very General Configuration +:PROPERTIES: +:header-args:matlab+: :tangle src/generateGeneralConfiguration.m +:header-args:matlab+: :comments none :mkdirp yes :eval no +:END: +<> + +This Matlab function is accessible [[file:src/generateGeneralConfiguration.m][here]]. + +** Function description +#+begin_src matlab + function [stewart] = generateGeneralConfiguration(stewart, args) + % generateGeneralConfiguration - Generate a Very General Configuration + % + % Syntax: [stewart] = generateGeneralConfiguration(stewart, args) + % + % Inputs: + % - stewart - A structure with the following fields + % - H [1x1] - Total height of the platform [m] + % - args - Can have the following fields: + % - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] + % - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] + % - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] + % - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] + % - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] + % - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] + % + % 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} +#+end_src + +** 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. + +** Optional Parameters +#+begin_src matlab + arguments + stewart + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3; + args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180); + end +#+end_src + +** Compute the pose +#+begin_src matlab + stewart.Fa = zeros(3,6); + stewart.Mb = zeros(3,6); +#+end_src + +#+begin_src matlab + 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]; + end +#+end_src + * =computeJointsPose=: Compute the Pose of the Joints :PROPERTIES: :header-args:matlab+: :tangle src/computeJointsPose.m diff --git a/src/generateGeneralConfiguration.m b/src/generateGeneralConfiguration.m new file mode 100644 index 0000000..99f9afa --- /dev/null +++ b/src/generateGeneralConfiguration.m @@ -0,0 +1,38 @@ +function [stewart] = generateGeneralConfiguration(stewart, args) +% generateGeneralConfiguration - Generate a Very General Configuration +% +% Syntax: [stewart] = generateGeneralConfiguration(stewart, args) +% +% Inputs: +% - stewart - A structure with the following fields +% - H [1x1] - Total height of the platform [m] +% - args - Can have the following fields: +% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m] +% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m] +% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad] +% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m] +% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m] +% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad] +% +% 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} + +arguments + stewart + args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.FR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3; + args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180); + args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 + args.MR (1,1) double {mustBeNumeric, mustBePositive} = 70e-3; + 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); + +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]; +end