-
Notifications
You must be signed in to change notification settings - Fork 3
/
robot_plotter.m
53 lines (45 loc) · 1.23 KB
/
robot_plotter.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
function fig = robot_plotter(robot,th,pe,pr,wr)
%% user data
rob_lk_thick = 2;
rob_hg_size = 2;
rob_ws = pr.ws;
nrob = length(robot);
%% robot plot
for i=1:nrob
pbe = PlanarRevolute.getFkine(robot(i).link,th(i,:));
pb(i,:) = pe(i,:)-pbe;
hrob(i) = robot(i).plot(th(i,:),q(SE2(pb(i,:))),'workspace',rob_ws,...
'dim',length(rob_ws)/2,'plat','hgsize',rob_hg_size,...
'lkthick', rob_lk_thick); hold on
end
%% reference trajectory
hpr = plot(pr.p(:,1),pr.p(:,2),'k','LineWidth',1);
%% centroid trajectory
pc = sum(pe)/nrob;
hpc = plot(pc(:,1),pc(:,2),'m*','LineWidth',1);
%% formation plot
% object shape
ipe = convhull_(pe(:,1:2));
hpe = patch(pe(ipe,1),pe(ipe,2),[0.7, 0.7, 0.7],'EdgeColor',[0.5,0.5,0.5],'LineWidth',2);
% hpe = plot(pe(ipe,1),pe(ipe,2),'Color',[0.5,0.5,0.5],'LineWidth',2);
% formation shape
ipb = convhull_(pb(:,1:2));
hpb = plot(pb(ipb,1),pb(ipb,2),'r--','LineWidth',2);
%% working range plot
if nargin==5
for i=1:nrob
hwr(i) = patch('Faces',1:size(wr,1),'Vertices',wr(:,:,i),...
'LineStyle','none','FaceColor','g','FaceAlpha',0.3);
end
else
hwr = [];
end
hold off
%% save data
fig.hrob = hrob;
fig.hpr = hpr;
fig.hpc = hpc;
fig.hpe = hpe;
fig.hpb = hpb;
fig.hwr = hwr;
end