-
Notifications
You must be signed in to change notification settings - Fork 11
/
ncgr_plot.m
104 lines (83 loc) · 2.94 KB
/
ncgr_plot.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
function g = ncgr_plot(g, r, view_vector, axis_scale, x_range, y_range, z_range)
% Plot the robot, set the view vector, set the scale the three arrows
% representing the link's coordinate frame, and set the limits of the
% current axes
%
% Inputs:
% g - graphic structure.
% r - robot structure.
% view_vector - view vector (1x3 vector) - optional
% x_range - x-axis limits (1x2 vector) - optional
% y_range - x-axis limits (1x3 vector) - optional
% z_range - x-axis limits (1x3 vector) - optional
%
% Outputs:
% g - updated graphic structure
%
% Contact: manurung.auralius@gmail.com
%
global N_DOFS;
max_reach = 1.1*max(sum(abs(r.d)), sum(abs(r.a)));
if (g.h == -1) % only do once
figure;
hold on;
grid on;
axis equal
if nargin < 3
view_vector = [1 1 1];
axis_scale = 0.5;
xlim([r.base(1)-max_reach r.base(1)+max_reach]);
ylim([r.base(2)-max_reach r.base(2)+max_reach]);
zlim([r.base(3)-max_reach r.base(3)+max_reach]);
elseif nargin < 4
axis_scale = 0.5;
xlim([r.base(1)-max_reach r.base(1)+max_reach]);
ylim([r.base(2)-max_reach r.base(2)+max_reach]);
zlim([r.base(3)-max_reach r.base(3)+max_reach]);
elseif nargin < 5
xlim([r.base(1)-max_reach r.base(1)+max_reach]);
ylim([r.base(2)-max_reach r.base(2)+max_reach]);
zlim([r.base(3)-max_reach r.base(3)+max_reach]);
elseif nargin < 6
xlim(x_range);
ylim([r.base(2)-max_reach r.base(2)+max_reach]);
zlim([r.base(3)-max_reach r.base(3)+max_reach]);
elseif nargin < 7
xlim(x_range);
ylim(y_range);
zlim([r.base(3)-max_reach r.base(3)+max_reach]);
else
xlim(x_range);
ylim(y_range);
zlim(z_range);
end
view(view_vector);
g.h = plot3(0, 0, 0, '-m*', 'LineWidth', 4);
g.quiver_x = quiver3(0,0,0,0,0,0, axis_scale, 'r');
g.quiver_y = quiver3(0,0,0,0,0,0, axis_scale, 'g');
g.quiver_z = quiver3(0,0,0,0,0,0, axis_scale, 'b');
xlabel('x');
ylabel('y');
zlabel('z');
for i = 0 : N_DOFS
g.htxt(i+1) = text(0,0,0, num2str(i), 'FontWeight', 'bold');
end
set(g.htxt(1), 'Position', r.base);
end
for i = 1 : N_DOFS
vx(:, i) = r.T(1:3, 1:3, i) * [1; 0; 0];
vy(:, i) = r.T(1:3, 1:3, i) * [0; 1; 0];
vz(:, i) = r.T(1:3, 1:3, i) * [0; 0; 1];
x(:, i) = r.T(1:3, 4 , i);
set(g.htxt(i+1), 'Position', x(:, i) + [0; 0; 0.2]);
end
set(g.h, 'XData', [r.base(1) x(1, :)], 'YData', [r.base(2) x(2, :)], ...
'ZData', [r.base(3) x(3, :)]);
set(g.quiver_x, 'XData', x(1, :), 'YData', x(2, :), 'ZData', x(3, :), ...
'UData', vx(1,:), 'VData', vx(2,:), 'WData', vx(3,:));
set(g.quiver_y, 'XData', x(1, :), 'YData', x(2, :), 'ZData', x(3, :), ...
'UData', vy(1,:), 'VData', vy(2,:), 'WData', vy(3,:));
set(g.quiver_z, 'XData', x(1, :), 'YData', x(2, :), 'ZData', x(3, :), ...
'UData', vz(1,:), 'VData', vz(2,:), 'WData', vz(3,:));
drawnow;
end