-
Notifications
You must be signed in to change notification settings - Fork 0
/
plot_roboarm_trajectories.m
executable file
·71 lines (54 loc) · 1.78 KB
/
plot_roboarm_trajectories.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
function plot_roboarm_trajectories(U,starting_pos,colors,xs,ys,xg,yg,xy,zact,rrew)
N = sqrt(size(U,2));
figure(1)
subplot(312)
scatter(xy(:,1),xy(:,2),10,exp(rrew),'fill','s')%U(:,state),'fill')
hold on
axis equal
axis off
subplot(313)
scatter(xy(:,1),xy(:,2),10,log(zact),'fill','s')%U(:,state),'fill')
hold on
for sp = 1:size(starting_pos,1)
grid = zeros(N,N);
grid(starting_pos(sp,1),starting_pos(sp,2)) = 1;
statehist = compute_maximal_trajectory(U,find(grid(:)==1));
subplot(313)
plot(xy(statehist,1),xy(statehist,2),'Color',colors(sp,:),'linewidth',2,'MarkerSize',25);
hold on
scatter(xy(statehist(1),1), xy(statehist(1),2),70,[0 1 0],'fill')
hold on
scatter(xy(statehist(end),1), xy(statehist(end),2),70,[1 0 0],'fill')
hold on
axis equal
axis off
% Plot as strobed arm trajectory
draw_arm = true;
if draw_arm
subplot(311)
if sp == 1
rectangle('Position',[-2 .9 4 .2],'FaceColor','k','EdgeColor','none')
hold on
end
for i = 1:length(statehist)
if i == 1
c = [0 1 0];
elseif i == length(statehist)
c = [1 0 0];
else
c = colors(sp,:);
end
plot([0 xs(statehist(i)) xg(statehist(i))],[0 ys(statehist(i)) yg(statehist(i))],'Color',colors(sp,:),'linewidth',.5)
if i == length(statehist)
scatter(xg(statehist(i)), yg(statehist(i)),70,c,'fill')
end
hold on
end
scatter(xg(statehist(1)), yg(statehist(1)),70,[0 1 0],'fill')
%plot(xg(goal_line),yg(goal_line),'ro','MarkerSize',5)
xlim([-2.5 2.5])
ylim([-2 2])
axis equal
axis off
end
end