Add color options to display the stewart platform

This commit is contained in:
Thomas Dehaeze 2020-02-07 17:18:46 +01:00
parent 69130d746c
commit 0dc551e2b2
3 changed files with 326 additions and 308 deletions

View File

@ -9,6 +9,9 @@ function [] = displayArchitecture(stewart, args)
% - 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}
% - F_color [color] - Color used for the Fixed elements
% - M_color [color] - Color used for the Mobile elements
% - L_color [color] - Color used for the Legs elements
% - frames [true/false] - Display the Frames
% - legs [true/false] - Display the Legs
% - joints [true/false] - Display the Joints
@ -21,6 +24,9 @@ arguments
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
args.F_color = [0 0.4470 0.7410]
args.M_color = [0.8500 0.3250 0.0980]
args.L_color = [0 0 0]
args.frames logical {mustBeNumericOrLogical} = true
args.legs logical {mustBeNumericOrLogical} = true
args.joints logical {mustBeNumericOrLogical} = true
@ -47,12 +53,12 @@ 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])
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Ff(1) + d_label, ...
Ff(2) + d_label, ...
Ff(3) + d_label, '$\{F\}$', 'Color', [0 0.4470 0.7410]);
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
end
end
@ -60,12 +66,12 @@ 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])
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Fa(1) + d_label, ...
Fa(2) + d_label, ...
Fa(3) + d_label, '$\{A\}$', 'Color', [0 0.4470 0.7410]);
Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color);
end
end
@ -79,18 +85,18 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', [0 0.4470 0.7410]);
points(3,:), '-', 'Color', args.F_color);
end
if args.joints
scatter3(stewart.Fa(1,:), ...
stewart.Fa(2,:), ...
stewart.Fa(3,:), 'MarkerEdgeColor', [0 0.4470 0.7410]);
stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color);
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]);
stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
end
end
end
@ -100,12 +106,12 @@ 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])
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(Fm(1) + d_label, ...
Fm(2) + d_label, ...
Fm(3) + d_label, '$\{M\}$', 'Color', [0.8500 0.3250 0.0980]);
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
end
end
@ -114,12 +120,12 @@ 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])
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(FB(1) - d_label, ...
FB(2) + d_label, ...
FB(3) + d_label, '$\{B\}$', 'Color', [0.8500 0.3250 0.0980]);
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
end
end
@ -133,7 +139,7 @@ if args.platforms && isfield(stewart, 'platforms') && isfield(stewart.platforms,
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', [0.8500 0.3250 0.0980]);
points(3,:), '-', 'Color', args.M_color);
end
if args.joints
@ -141,13 +147,13 @@ if args.joints
scatter3(Fb(1,:), ...
Fb(2,:), ...
Fb(3,:), 'MarkerEdgeColor', [0.8500 0.3250 0.0980]);
Fb(3,:), 'MarkerEdgeColor', args.M_color);
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]);
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
end
end
end
@ -156,12 +162,12 @@ 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-');
[stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
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');
(stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
end
end
end

File diff suppressed because it is too large Load Diff

View File

@ -950,6 +950,9 @@ This Matlab function is accessible [[file:src/displayArchitecture.m][here]].
% - 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}
% - F_color [color] - Color used for the Fixed elements
% - M_color [color] - Color used for the Mobile elements
% - L_color [color] - Color used for the Legs elements
% - frames [true/false] - Display the Frames
% - legs [true/false] - Display the Legs
% - joints [true/false] - Display the Joints
@ -968,6 +971,9 @@ This Matlab function is accessible [[file:src/displayArchitecture.m][here]].
stewart
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
args.ARB (3,3) double {mustBeNumeric} = eye(3)
args.F_color = [0 0.4470 0.7410]
args.M_color = [0.8500 0.3250 0.0980]
args.L_color = [0 0 0]
args.frames logical {mustBeNumericOrLogical} = true
args.legs logical {mustBeNumericOrLogical} = true
args.joints logical {mustBeNumericOrLogical} = true
@ -1018,12 +1024,12 @@ 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])
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Ff(1) + d_label, ...
Ff(2) + d_label, ...
Ff(3) + d_label, '$\{F\}$', 'Color', [0 0.4470 0.7410]);
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
end
end
#+end_src
@ -1034,12 +1040,12 @@ Now plot the frame $\{A\}$ fixed to the Base.
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])
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
if args.labels
text(Fa(1) + d_label, ...
Fa(2) + d_label, ...
Fa(3) + d_label, '$\{A\}$', 'Color', [0 0.4470 0.7410]);
Fa(3) + d_label, '$\{A\}$', 'Color', args.F_color);
end
end
#+end_src
@ -1056,7 +1062,7 @@ Let's then plot the circle corresponding to the shape of the Fixed base.
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', [0 0.4470 0.7410]);
points(3,:), '-', 'Color', args.F_color);
end
#+end_src
@ -1065,12 +1071,12 @@ 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]);
stewart.Fa(3,:), 'MarkerEdgeColor', args.F_color);
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]);
stewart.Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
end
end
end
@ -1088,12 +1094,12 @@ Plot the frame $\{M\}$.
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])
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(Fm(1) + d_label, ...
Fm(2) + d_label, ...
Fm(3) + d_label, '$\{M\}$', 'Color', [0.8500 0.3250 0.0980]);
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
end
end
#+end_src
@ -1105,12 +1111,12 @@ Plot the frame $\{B\}$.
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])
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
if args.labels
text(FB(1) - d_label, ...
FB(2) + d_label, ...
FB(3) + d_label, '$\{B\}$', 'Color', [0.8500 0.3250 0.0980]);
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
end
end
#+end_src
@ -1127,7 +1133,7 @@ Let's then plot the circle corresponding to the shape of the Mobile platform.
plot3(points(1,:), ...
points(2,:), ...
points(3,:), '-', 'Color', [0.8500 0.3250 0.0980]);
points(3,:), '-', 'Color', args.M_color);
end
#+end_src
@ -1138,13 +1144,13 @@ Plot the position and labels of the rotation joints fixed to the mobile platform
scatter3(Fb(1,:), ...
Fb(2,:), ...
Fb(3,:), 'MarkerEdgeColor', [0.8500 0.3250 0.0980]);
Fb(3,:), 'MarkerEdgeColor', args.M_color);
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]);
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
end
end
end
@ -1160,12 +1166,12 @@ Plot the legs connecting the joints of the fixed base to the joints of the mobil
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-');
[stewart.Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
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');
(stewart.Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
end
end
end