diff --git a/figs/stewart_architecture_example.pdf b/figs/stewart_architecture_example.pdf new file mode 100644 index 0000000..ab3e103 Binary files /dev/null and b/figs/stewart_architecture_example.pdf differ diff --git a/figs/stewart_architecture_example.png b/figs/stewart_architecture_example.png new file mode 100644 index 0000000..1d81f4d Binary files /dev/null and b/figs/stewart_architecture_example.png differ diff --git a/figs/stewart_architecture_example_pose.pdf b/figs/stewart_architecture_example_pose.pdf new file mode 100644 index 0000000..2173ca7 Binary files /dev/null and b/figs/stewart_architecture_example_pose.pdf differ diff --git a/figs/stewart_architecture_example_pose.png b/figs/stewart_architecture_example_pose.png new file mode 100644 index 0000000..cf238a9 Binary files /dev/null and b/figs/stewart_architecture_example_pose.png differ diff --git a/src/displayArchitecture.m b/src/displayArchitecture.m new file mode 100644 index 0000000..3aa2073 --- /dev/null +++ b/src/displayArchitecture.m @@ -0,0 +1,171 @@ +function [] = displayArchitecture(stewart, args) +% displayArchitecture - 3D plot of the Stewart platform architecture +% +% Syntax: [] = displayArchitecture(args) +% +% Inputs: +% - stewart +% - args - Structure with 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} +% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A} +% - frames [true/false] - Display the Frames +% - legs [true/false] - Display the Legs +% - joints [true/false] - Display the Joints +% - labels [true/false] - Display the Labels +% - platforms [true/false] - Display the Platforms +% +% Outputs: + +arguments + stewart + args.AP (3,1) double {mustBeNumeric} = zeros(3,1) + args.ARB (3,3) double {mustBeNumeric} = eye(3) + args.frames logical {mustBeNumericOrLogical} = true + args.legs logical {mustBeNumericOrLogical} = true + args.joints logical {mustBeNumericOrLogical} = true + args.labels logical {mustBeNumericOrLogical} = true + args.platforms logical {mustBeNumericOrLogical} = true +end + +figure; +hold on; + +FTa = [eye(3), stewart.FO_A; ... + zeros(1,3), 1]; +ATb = [args.ARB, args.AP; ... + zeros(1,3), 1]; +BTm = [eye(3), -stewart.MO_B; ... + zeros(1,3), 1]; + +FTm = FTa*ATb*BTm; + +d_unit_vector = stewart.H/4; + +d_label = stewart.H/20; + +Ff = [0, 0, 0]; +if args.frames + quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ... + [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410]) + + if args.labels + text(Ff(1) + d_label, ... + Ff(2) + d_label, ... + Ff(3) + d_label, '$\{F\}$', 'Color', [0 0.4470 0.7410]); + end +end + +Fa = stewart.FO_A; + +if args.frames + quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ... + [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410]) + + if args.labels + text(Fa(1) + d_label, ... + Fa(2) + d_label, ... + Fa(3) + d_label, '$\{A\}$', 'Color', [0 0.4470 0.7410]); + end +end + +if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr') + theta = [0:0.01:2*pi+0.01]; % Angles [rad] + v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal + center = [0; 0; 0]; % Center of the circle + radius = stewart.platforms.Fpr; % Radius of the circle [m] + + points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); + + plot3(points(1,:), ... + points(2,:), ... + points(3,:), '-', 'Color', [0 0.4470 0.7410]); +end + +if args.joints + scatter3(stewart.Fa(1,:), ... + stewart.Fa(2,:), ... + stewart.Fa(3,:), 'MarkerEdgeColor', [0 0.4470 0.7410]); + if args.labels + for i = 1:size(stewart.Fa,2) + text(stewart.Fa(1,i) + d_label, ... + stewart.Fa(2,i), ... + stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', [0 0.4470 0.7410]); + end + end +end + +Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F} + +if args.frames + FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors + quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ... + FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980]) + + if args.labels + text(Fm(1) + d_label, ... + Fm(2) + d_label, ... + Fm(3) + d_label, '$\{M\}$', 'Color', [0.8500 0.3250 0.0980]); + end +end + +FB = stewart.FO_A + args.AP; + +if args.frames + FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors + quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ... + FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980]) + + if args.labels + text(FB(1) - d_label, ... + FB(2) + d_label, ... + FB(3) + d_label, '$\{B\}$', 'Color', [0.8500 0.3250 0.0980]); + end +end + +if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr') + theta = [0:0.01:2*pi+0.01]; % Angles [rad] + v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal + center = Fm(1:3); % Center of the circle + radius = stewart.platforms.Mpr; % Radius of the circle [m] + + points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta)); + + plot3(points(1,:), ... + points(2,:), ... + points(3,:), '-', 'Color', [0.8500 0.3250 0.0980]); +end + +if args.joints + Fb = FTm*[stewart.Mb;ones(1,6)]; + + scatter3(Fb(1,:), ... + Fb(2,:), ... + Fb(3,:), 'MarkerEdgeColor', [0.8500 0.3250 0.0980]); + + if args.labels + for i = 1:size(Fb,2) + text(Fb(1,i) + d_label, ... + Fb(2,i), ... + Fb(3,i), sprintf('$b_{%i}$', i), 'Color', [0.8500 0.3250 0.0980]); + end + end +end + +if args.legs + for i = 1:6 + plot3([stewart.Fa(1,i), Fb(1,i)], ... + [stewart.Fa(2,i), Fb(2,i)], ... + [stewart.Fa(3,i), Fb(3,i)], 'k-'); + + if args.labels + text((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ... + (stewart.Fa(2,i)+Fb(2,i))/2, ... + (stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', 'k'); + end + end +end + +view([1 -0.6 0.4]); +axis equal; +axis off; diff --git a/stewart-architecture.html b/stewart-architecture.html index db59585..5bb168c 100644 --- a/stewart-architecture.html +++ b/stewart-architecture.html @@ -4,7 +4,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> - + Stewart Platform - Definition of the Architecture @@ -268,93 +268,104 @@ for the JavaScript code in this tag.

Table of Contents

The obtained stewart Matlab structure contains all the information for analysis of the Stewart platform and for simulations using Simscape.

+ +

+The function displayArchitecture can be used to display the current Stewart configuration: +

+
+
displayArchitecture(stewart);
+
+
+ + +
+

stewart_architecture_example.png +

+

Figure 5: Display of the current Stewart platform architecture (png, pdf)

+
+ +

+There are many options to show or hides elements such as labels and frames. +The documentation of the function is available here. +

+ +

+Let’s now move a little bit the top platform and re-display the configuration: +

+
+
tx = 0.1; % [rad]
+ty = 0.2; % [rad]
+tz = 0.05; % [rad]
+
+Rx = [1 0        0;
+      0 cos(tx) -sin(tx);
+      0 sin(tx)  cos(tx)];
+
+Ry = [ cos(ty) 0 sin(ty);
+      0        1 0;
+      -sin(ty) 0 cos(ty)];
+
+Rz = [cos(tz) -sin(tz) 0;
+      sin(tz)  cos(tz) 0;
+      0        0       1];
+
+ARB = Rz*Ry*Rx;
+AP = [0.08; 0; 0]; % [m]
+
+displayArchitecture(stewart, 'AP', AP, 'ARB', ARB);
+view([0 -1 0]);
+
+
+ + +
+

stewart_architecture_example_pose.png +

+

Figure 6: Display of the Stewart platform architecture at some defined pose (png, pdf)

+
-
-

5 Functions

+
+

5 Functions

- +

-
-

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

+
+

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

- +

@@ -732,9 +798,9 @@ This Matlab function is accessible her

-
-

Function description

-
+
+

Function description

+
function [stewart] = initializeFramesPositions(args)
 % initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
@@ -757,21 +823,21 @@ This Matlab function is accessible her
-
-

Documentation

-
+
+

Documentation

+
-
+

stewart-frames-position.png

-

Figure 5: Definition of the position of the frames

+

Figure 7: Definition of the position of the frames

-
-

Optional Parameters

-
+
+

Optional Parameters

+
arguments
     args.H    (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
@@ -782,9 +848,9 @@ This Matlab function is accessible her
-
-

Initialize the Stewart structure

-
+
+

Initialize the Stewart structure

+
stewart = struct();
@@ -792,9 +858,9 @@ This Matlab function is accessible her
-
-

Compute the position of each frame

-
+
+

Compute the position of each frame

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

5.2 generateGeneralConfiguration: Generate a Very General Configuration

+
+

5.2 generateGeneralConfiguration: Generate a Very General Configuration

- +

@@ -821,9 +887,9 @@ This Matlab function is accessible

-
-

Function description

-
+
+

Function description

+
function [stewart] = generateGeneralConfiguration(stewart, args)
 % generateGeneralConfiguration - Generate a Very General Configuration
@@ -848,26 +914,26 @@ This Matlab function is accessible
-
-

Documentation

-
+
+

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 (see Figure 6). +The radius of the circles can be chosen as well as the angles where the joints are located (see Figure 8).

-
+

stewart_bottom_plate.png

-

Figure 6: Position of the joints

+

Figure 8: Position of the joints

-
-

Optional Parameters

-
+
+

Optional Parameters

+
arguments
     stewart
@@ -883,9 +949,9 @@ The radius of the circles can be chosen as well as the angles where the joints a
-
-

Compute the pose

-
+
+

Compute the pose

+
stewart.Fa = zeros(3,6);
 stewart.Mb = zeros(3,6);
@@ -903,11 +969,11 @@ stewart.Mb = zeros(3,6);
-
-

5.3 computeJointsPose: Compute the Pose of the Joints

+
+

5.3 computeJointsPose: Compute the Pose of the Joints

- +

@@ -915,9 +981,9 @@ This Matlab function is accessible here.

-
-

Function description

-
+
+

Function description

+
function [stewart] = computeJointsPose(stewart)
 % computeJointsPose -
@@ -948,21 +1014,21 @@ This Matlab function is accessible here.
-
-

Documentation

-
+
+

Documentation

+
-
+

stewart-struts.png

-

Figure 7: Position and orientation of the struts

+

Figure 9: Position and orientation of the struts

-
-

Compute the position of the Joints

-
+
+

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]);
@@ -974,9 +1040,9 @@ stewart.Ba = stewart.Aa - repmat( stewart.MO_B
-
-

Compute the strut length and orientation

-
+
+

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
 
@@ -991,9 +1057,9 @@ stewart.l = vecnorm(stewart.Ab - stewart.Aa)
-
-

Compute the orientation of the Joints

-
+
+

Compute the orientation of the Joints

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

5.4 initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose

+
+

5.4 initializeStewartPose: Determine the initial stroke in each leg to have the wanted pose

- +

@@ -1023,9 +1089,9 @@ This Matlab function is accessible here

-
-

Function description

-
+
+

Function description

+
function [stewart] = initializeStewartPose(stewart, args)
 % initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
@@ -1049,9 +1115,9 @@ This Matlab function is accessible here
-
-

Optional Parameters

-
+
+

Optional Parameters

+
arguments
     stewart
@@ -1063,9 +1129,9 @@ This Matlab function is accessible here
-
-

Use the Inverse Kinematic function

-
+
+

Use the Inverse Kinematic function

+
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
 
@@ -1076,11 +1142,11 @@ stewart.dLi = dLi;
-
-

5.5 initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms

+
+

5.5 initializeCylindricalPlatforms: Initialize the geometry of the Fixed and Mobile Platforms

- +

@@ -1088,9 +1154,9 @@ This Matlab function is accessible -

Function description

-
+
+

Function description

+
function [stewart] = initializeCylindricalPlatforms(stewart, args)
 % initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
@@ -1122,9 +1188,9 @@ This Matlab function is accessible 
-
Optional Parameters

-

+

+
Optional Parameters

+

 

 
arguments
     stewart
@@ -1140,9 +1206,9 @@ This Matlab function is accessible 
-
Create the platforms struct

-

+

+
Create the platforms struct

+

 

 
platforms = struct();
 
@@ -1164,9 +1230,9 @@ platforms.Mpi = diag([1/12 
 

 

 
-

-
Save the platforms struct

-

+

+
Save the platforms struct

+

 

 
stewart.platforms = platforms;
 

@@ -1175,11 +1241,11 @@ platforms.Mpi = diag([1/12 
 

 

 
-

-
5.6 initializeCylindricalStruts: Define the inertia of cylindrical struts

+

+
5.6 initializeCylindricalStruts: Define the inertia of cylindrical struts

 

 

-
+
 

 
 

@@ -1187,9 +1253,9 @@ This Matlab function is accessible h
 

 

 
-

-
Function description

-

+

+
Function description

+

 

 
function [stewart] = initializeCylindricalStruts(stewart, args)
 % initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
@@ -1221,9 +1287,9 @@ This Matlab function is accessible h
 

 

 
-

-
Optional Parameters

-

+

+
Optional Parameters

+

 

 
arguments
     stewart
@@ -1239,9 +1305,9 @@ This Matlab function is accessible h
 

 

 
-

-
Create the struts structure

-

+

+
Create the struts structure

+

 

 
struts = struct();
 
@@ -1274,11 +1340,11 @@ struts.Msi = zeros(3, 3, 6);
 

 

 
-

-
5.7 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

+

+
5.7 initializeStrutDynamics: Add Stiffness and Damping properties of each strut

 

 

-
+
 

 
 

@@ -1286,9 +1352,9 @@ This Matlab function is accessible here<
 

 

 
-

-
Function description

-

+

+
Function description

+

 

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

 

 
-

-
Optional Parameters

-

+

+
Optional Parameters

+

 

 
arguments
     stewart
@@ -1323,9 +1389,9 @@ This Matlab function is accessible here<
 

 

 
-

-
Add Stiffness and Damping properties of each strut

-

+

+
Add Stiffness and Damping properties of each strut

+

 

 
stewart.Ki = args.Ki;
 stewart.Ci = args.Ci;
@@ -1335,11 +1401,11 @@ stewart.Ci = args.Ci;
 

 

 
-

-
5.8 initializeJointDynamics: Add Stiffness and Damping properties for spherical joints

+

+
5.8 initializeJointDynamics: Add Stiffness and Damping properties for spherical joints

 

 

-
+
 

 
 

@@ -1347,9 +1413,9 @@ This Matlab function is accessible here<
 

 

 
-

-
Function description

-

+

+
Function description

+

 

 
function [stewart] = initializeJointDynamics(stewart, args)
 % initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
@@ -1379,9 +1445,9 @@ This Matlab function is accessible here<
 

 

 
-

-
Optional Parameters

-

+

+
Optional Parameters

+

 

 
arguments
     stewart
@@ -1398,9 +1464,9 @@ This Matlab function is accessible here<
 

 

 
-

-
Add Stiffness and Damping properties of each strut

-

+
+
+

+
5.9 displayArchitecture: 3D plot of the Stewart platform architecture

+

+

+
+

+
+

+This Matlab function is accessible here.
+

+

+
+

+
Function description

+

+

+
function [] = displayArchitecture(stewart, args)
+% displayArchitecture - 3D plot of the Stewart platform architecture
+%
+% Syntax: [] = displayArchitecture(args)
+%
+% Inputs:
+%    - stewart
+%    - args - Structure with 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}
+%        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+%        - frames    [true/false] - Display the Frames
+%        - legs      [true/false] - Display the Legs
+%        - joints    [true/false] - Display the Joints
+%        - labels    [true/false] - Display the Labels
+%        - platforms [true/false] - Display the Platforms
+%
+% Outputs:
+

+

+

+

+
+

+
Optional Parameters

+

+

+
arguments
+    stewart
+    args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+    args.ARB (3,3) double {mustBeNumeric} = eye(3)
+    args.frames logical {mustBeNumericOrLogical} = true
+    args.legs logical {mustBeNumericOrLogical} = true
+    args.joints logical {mustBeNumericOrLogical} = true
+    args.labels logical {mustBeNumericOrLogical} = true
+    args.platforms logical {mustBeNumericOrLogical} = true
+end
+

+

+

+

+
+

+
Figure Creation, Frames and Homogeneous transformations

+

+

+The reference frame of the 3d plot corresponds to the frame \(\{F\}\).
+

+

+
figure;
+hold on;
+

+

+
+

+We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is \(\{F\}\).
+

+

+
FTa = [eye(3), stewart.FO_A; ...
+       zeros(1,3), 1];
+ATb = [args.ARB, args.AP; ...
+       zeros(1,3), 1];
+BTm = [eye(3), -stewart.MO_B; ...
+       zeros(1,3), 1];
+
+FTm = FTa*ATb*BTm;
+

+

+
+

+Let’s define a parameter that define the length of the unit vectors used to display the frames.
+

+

+
d_unit_vector = stewart.H/4;
+

+

+
+

+Let’s define a parameter used to position the labels with respect to the center of the element.
+

+

+
d_label = stewart.H/20;
+

+

+

+

+
+

+
Fixed Base elements

+

+

+Let’s first plot the frame \(\{F\}\).
+

+

+
Ff = [0, 0, 0];
+if args.frames
+  quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
+          [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410])
+
+  if args.labels
+    text(Ff(1) + d_label, ...
+        Ff(2) + d_label, ...
+        Ff(3) + d_label, '$\{F\}$', 'Color', [0 0.4470 0.7410]);
+  end
+end
+

+

+
+

+Now plot the frame \(\{A\}\) fixed to the Base.
+

+

+
Fa = stewart.FO_A;
+
+if args.frames
+  quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ...
+          [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410])
+
+  if args.labels
+    text(Fa(1) + d_label, ...
+         Fa(2) + d_label, ...
+         Fa(3) + d_label, '$\{A\}$', 'Color', [0 0.4470 0.7410]);
+  end
+end
+

+

+
+

+Let’s then plot the circle corresponding to the shape of the Fixed base.
+

+

+
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr')
+  theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+  v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
+  center = [0; 0; 0]; % Center of the circle
+  radius = stewart.platforms.Fpr; % Radius of the circle [m]
+
+  points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+  plot3(points(1,:), ...
+        points(2,:), ...
+        points(3,:), '-', 'Color', [0 0.4470 0.7410]);
+end
+

+

+
+

+Let’s now plot the position and labels of the Fixed Joints
+

+

+
if args.joints
+  scatter3(stewart.Fa(1,:), ...
+           stewart.Fa(2,:), ...
+           stewart.Fa(3,:), 'MarkerEdgeColor', [0 0.4470 0.7410]);
+  if args.labels
+    for i = 1:size(stewart.Fa,2)
+      text(stewart.Fa(1,i) + d_label, ...
+           stewart.Fa(2,i), ...
+           stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', [0 0.4470 0.7410]);
+    end
+  end
+end
+

+

+

+

+
+

+
Mobile Platform elements

+

+

+Plot the frame \(\{M\}\).
+

+

+
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
+
+if args.frames
+  FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+  quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
+          FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980])
+
+  if args.labels
+    text(Fm(1) + d_label, ...
+         Fm(2) + d_label, ...
+         Fm(3) + d_label, '$\{M\}$', 'Color', [0.8500 0.3250 0.0980]);
+  end
+end
+

+

+
+

+Plot the frame \(\{B\}\).
+

+

+
FB = stewart.FO_A + args.AP;
+
+if args.frames
+  FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+  quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
+          FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980])
+
+  if args.labels
+    text(FB(1) - d_label, ...
+         FB(2) + d_label, ...
+         FB(3) + d_label, '$\{B\}$', 'Color', [0.8500 0.3250 0.0980]);
+  end
+end
+

+

+
+

+Let’s then plot the circle corresponding to the shape of the Mobile platform.
+

+

+
if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr')
+  theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+  v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
+  center = Fm(1:3); % Center of the circle
+  radius = stewart.platforms.Mpr; % Radius of the circle [m]
+
+  points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+  plot3(points(1,:), ...
+        points(2,:), ...
+        points(3,:), '-', 'Color', [0.8500 0.3250 0.0980]);
+end
+

+

+
+

+Plot the position and labels of the rotation joints fixed to the mobile platform.
+

+

+
if args.joints
+  Fb = FTm*[stewart.Mb;ones(1,6)];
+
+  scatter3(Fb(1,:), ...
+           Fb(2,:), ...
+           Fb(3,:), 'MarkerEdgeColor', [0.8500 0.3250 0.0980]);
+
+  if args.labels
+    for i = 1:size(Fb,2)
+      text(Fb(1,i) + d_label, ...
+           Fb(2,i), ...
+           Fb(3,i), sprintf('$b_{%i}$', i), 'Color', [0.8500 0.3250 0.0980]);
+    end
+  end
+end
+

+

+

+

+
+

+
Legs

+

+

+Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
+

+

+
if args.legs
+  for i = 1:6
+    plot3([stewart.Fa(1,i), Fb(1,i)], ...
+          [stewart.Fa(2,i), Fb(2,i)], ...
+          [stewart.Fa(3,i), Fb(3,i)], 'k-');
+
+    if args.labels
+      text((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ...
+           (stewart.Fa(2,i)+Fb(2,i))/2, ...
+           (stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', 'k');
+    end
+  end
+end
+

+

+

+

+
+

+
5.9.1 Figure parameters

+

+

+
view([1 -0.6 0.4]);
+axis equal;
+axis off;
+

+

+

+

+

 

 
 

@@ -1433,7 +1806,7 @@ This Matlab function is accessible here<
 

 

 
Author: Dehaeze Thomas

-
Created: 2020-02-06 jeu. 18:23

+
Created: 2020-02-07 ven. 17:11

 

 
 
diff --git a/stewart-architecture.org b/stewart-architecture.org
index 86d8eae..a63dbb4 100644
--- a/stewart-architecture.org
+++ b/stewart-architecture.org
@@ -221,11 +221,62 @@ Then, define the inertia and geometry of the fixed base, mobile platform and str
 Finally, initialize the strut stiffness and damping properties.
 #+begin_src matlab
   stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
-  stewart = initializeJointDynamics(stewart, 'Ksi', zeros(6,1), 'Csi', zeros(6,1));
+  stewart = initializeJointDynamics(stewart);
 #+end_src
 
 The obtained =stewart= Matlab structure contains all the information for analysis of the Stewart platform and for simulations using Simscape.
 
+The function =displayArchitecture= can be used to display the current Stewart configuration:
+#+begin_src matlab
+  displayArchitecture(stewart);
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/stewart_architecture_example.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
+<>
+#+end_src
+
+#+name: fig:stewart_architecture_example
+#+caption: Display of the current Stewart platform architecture ([[./figs/stewart_architecture_example.png][png]], [[./figs/stewart_architecture_example.pdf][pdf]])
+[[file:figs/stewart_architecture_example.png]]
+
+There are many options to show or hides elements such as labels and frames.
+The documentation of the function is available [[sec:displayArchitecture][here]].
+
+Let's now move a little bit the top platform and re-display the configuration:
+#+begin_src matlab
+  tx = 0.1; % [rad]
+  ty = 0.2; % [rad]
+  tz = 0.05; % [rad]
+
+  Rx = [1 0        0;
+        0 cos(tx) -sin(tx);
+        0 sin(tx)  cos(tx)];
+
+  Ry = [ cos(ty) 0 sin(ty);
+        0        1 0;
+        -sin(ty) 0 cos(ty)];
+
+  Rz = [cos(tz) -sin(tz) 0;
+        sin(tz)  cos(tz) 0;
+        0        0       1];
+
+  ARB = Rz*Ry*Rx;
+  AP = [0.08; 0; 0]; % [m]
+
+  displayArchitecture(stewart, 'AP', AP, 'ARB', ARB);
+  view([0 -1 0]);
+#+end_src
+
+#+header: :tangle no :exports results :results none :noweb yes
+#+begin_src matlab :var filepath="figs/stewart_architecture_example_pose.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
+<>
+#+end_src
+
+#+name: fig:stewart_architecture_example_pose
+#+caption: Display of the Stewart platform architecture at some defined pose ([[./figs/stewart_architecture_example_pose.png][png]], [[./figs/stewart_architecture_example_pose.pdf][pdf]])
+[[file:figs/stewart_architecture_example_pose.png]]
+
 * Functions
 <>
 ** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
@@ -874,6 +925,259 @@ This Matlab function is accessible [[file:src/initializeJointDynamics.m][here]].
   end
 #+end_src
 
+** =displayArchitecture=: 3D plot of the Stewart platform architecture
+:PROPERTIES:
+:header-args:matlab+: :tangle src/displayArchitecture.m
+:header-args:matlab+: :comments none :mkdirp yes :eval no
+:END:
+<>
+
+This Matlab function is accessible [[file:src/displayArchitecture.m][here]].
+
+*** Function description
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  function [] = displayArchitecture(stewart, args)
+  % displayArchitecture - 3D plot of the Stewart platform architecture
+  %
+  % Syntax: [] = displayArchitecture(args)
+  %
+  % Inputs:
+  %    - stewart
+  %    - args - Structure with 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}
+  %        - ARB  [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
+  %        - frames    [true/false] - Display the Frames
+  %        - legs      [true/false] - Display the Legs
+  %        - joints    [true/false] - Display the Joints
+  %        - labels    [true/false] - Display the Labels
+  %        - platforms [true/false] - Display the Platforms
+  %
+  % Outputs:
+#+end_src
+
+*** Optional Parameters
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+#+begin_src matlab
+  arguments
+      stewart
+      args.AP  (3,1) double {mustBeNumeric} = zeros(3,1)
+      args.ARB (3,3) double {mustBeNumeric} = eye(3)
+      args.frames logical {mustBeNumericOrLogical} = true
+      args.legs logical {mustBeNumericOrLogical} = true
+      args.joints logical {mustBeNumericOrLogical} = true
+      args.labels logical {mustBeNumericOrLogical} = true
+      args.platforms logical {mustBeNumericOrLogical} = true
+  end
+#+end_src
+
+*** Figure Creation, Frames and Homogeneous transformations
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+The reference frame of the 3d plot corresponds to the frame $\{F\}$.
+#+begin_src matlab
+  figure;
+  hold on;
+#+end_src
+
+We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
+#+begin_src matlab
+  FTa = [eye(3), stewart.FO_A; ...
+         zeros(1,3), 1];
+  ATb = [args.ARB, args.AP; ...
+         zeros(1,3), 1];
+  BTm = [eye(3), -stewart.MO_B; ...
+         zeros(1,3), 1];
+
+  FTm = FTa*ATb*BTm;
+#+end_src
+
+Let's define a parameter that define the length of the unit vectors used to display the frames.
+#+begin_src matlab
+  d_unit_vector = stewart.H/4;
+#+end_src
+
+Let's define a parameter used to position the labels with respect to the center of the element.
+#+begin_src matlab
+  d_label = stewart.H/20;
+#+end_src
+
+*** Fixed Base elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Let's first plot the frame $\{F\}$.
+#+begin_src matlab
+  Ff = [0, 0, 0];
+  if args.frames
+    quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
+            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410])
+
+    if args.labels
+      text(Ff(1) + d_label, ...
+          Ff(2) + d_label, ...
+          Ff(3) + d_label, '$\{F\}$', 'Color', [0 0.4470 0.7410]);
+    end
+  end
+#+end_src
+
+Now plot the frame $\{A\}$ fixed to the Base.
+#+begin_src matlab
+  Fa = stewart.FO_A;
+
+  if args.frames
+    quiver3(Fa(1)*ones(1,3), Fa(2)*ones(1,3), Fa(3)*ones(1,3), ...
+            [d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', [0 0.4470 0.7410])
+
+    if args.labels
+      text(Fa(1) + d_label, ...
+           Fa(2) + d_label, ...
+           Fa(3) + d_label, '$\{A\}$', 'Color', [0 0.4470 0.7410]);
+    end
+  end
+#+end_src
+
+Let's then plot the circle corresponding to the shape of the Fixed base.
+#+begin_src matlab
+  if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Fpr')
+    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+    v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
+    center = [0; 0; 0]; % Center of the circle
+    radius = stewart.platforms.Fpr; % Radius of the circle [m]
+
+    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+    plot3(points(1,:), ...
+          points(2,:), ...
+          points(3,:), '-', 'Color', [0 0.4470 0.7410]);
+  end
+#+end_src
+
+Let's now plot the position and labels of the Fixed Joints
+#+begin_src matlab
+  if args.joints
+    scatter3(stewart.Fa(1,:), ...
+             stewart.Fa(2,:), ...
+             stewart.Fa(3,:), 'MarkerEdgeColor', [0 0.4470 0.7410]);
+    if args.labels
+      for i = 1:size(stewart.Fa,2)
+        text(stewart.Fa(1,i) + d_label, ...
+             stewart.Fa(2,i), ...
+             stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', [0 0.4470 0.7410]);
+      end
+    end
+  end
+#+end_src
+
+*** Mobile Platform elements
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+
+Plot the frame $\{M\}$.
+#+begin_src matlab
+  Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
+
+  if args.frames
+    FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+    quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
+            FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980])
+
+    if args.labels
+      text(Fm(1) + d_label, ...
+           Fm(2) + d_label, ...
+           Fm(3) + d_label, '$\{M\}$', 'Color', [0.8500 0.3250 0.0980]);
+    end
+  end
+#+end_src
+
+Plot the frame $\{B\}$.
+#+begin_src matlab
+  FB = stewart.FO_A + args.AP;
+
+  if args.frames
+    FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
+    quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
+            FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', [0.8500 0.3250 0.0980])
+
+    if args.labels
+      text(FB(1) - d_label, ...
+           FB(2) + d_label, ...
+           FB(3) + d_label, '$\{B\}$', 'Color', [0.8500 0.3250 0.0980]);
+    end
+  end
+#+end_src
+
+Let's then plot the circle corresponding to the shape of the Mobile platform.
+#+begin_src matlab
+  if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms, 'Mpr')
+    theta = [0:0.01:2*pi+0.01]; % Angles [rad]
+    v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
+    center = Fm(1:3); % Center of the circle
+    radius = stewart.platforms.Mpr; % Radius of the circle [m]
+
+    points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
+
+    plot3(points(1,:), ...
+          points(2,:), ...
+          points(3,:), '-', 'Color', [0.8500 0.3250 0.0980]);
+  end
+#+end_src
+
+Plot the position and labels of the rotation joints fixed to the mobile platform.
+#+begin_src matlab
+  if args.joints
+    Fb = FTm*[stewart.Mb;ones(1,6)];
+
+    scatter3(Fb(1,:), ...
+             Fb(2,:), ...
+             Fb(3,:), 'MarkerEdgeColor', [0.8500 0.3250 0.0980]);
+
+    if args.labels
+      for i = 1:size(Fb,2)
+        text(Fb(1,i) + d_label, ...
+             Fb(2,i), ...
+             Fb(3,i), sprintf('$b_{%i}$', i), 'Color', [0.8500 0.3250 0.0980]);
+      end
+    end
+  end
+#+end_src
+
+*** Legs
+:PROPERTIES:
+:UNNUMBERED: t
+:END:
+Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
+#+begin_src matlab
+  if args.legs
+    for i = 1:6
+      plot3([stewart.Fa(1,i), Fb(1,i)], ...
+            [stewart.Fa(2,i), Fb(2,i)], ...
+            [stewart.Fa(3,i), Fb(3,i)], 'k-');
+
+      if args.labels
+        text((stewart.Fa(1,i)+Fb(1,i))/2 + d_label, ...
+             (stewart.Fa(2,i)+Fb(2,i))/2, ...
+             (stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', 'k');
+      end
+    end
+  end
+#+end_src
+
+*** Figure parameters
+#+begin_src matlab
+  view([1 -0.6 0.4]);
+  axis equal;
+  axis off;
+#+end_src
+
 * Bibliography                                                        :ignore:
 bibliographystyle:unsrt
 bibliography:ref.bib