Revision 1812
Fixes wheel speed noise, prettier visualization with scaled arrows, and always displays plots
| visualizeRobots.m | ||
|---|---|---|
| 1 |
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,idx,color,makeMovie,mov,winsize)
|
|
| 1 |
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,Speeds,idx,color,mov,winsize)
|
|
| 2 | 2 |
|
| 3 |
% Scales arrows to length |
|
| 4 |
arrowScale = 1.5; |
|
| 5 |
|
|
| 3 | 6 |
clf; |
| 4 | 7 |
ax = [min(min(Xs))-1, max(max(Xs))+1, min(min(Ys))-1, max(max(Ys))+1]; |
| 5 | 8 |
|
| ... | ... | |
| 23 | 26 |
axis manual |
| 24 | 27 |
set( gca,'nextplot','add' ); |
| 25 | 28 |
for j = 1:n |
| 26 |
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx)+cos(Thetas(j,idx)),Ys(j,idx)+sin(Thetas(j,idx)),...
|
|
| 29 |
h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + Speeds(j, idx-1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx-1) * arrowScale * sin(Thetas(j,idx)),...
|
|
| 27 | 30 |
'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) ); |
| 28 |
set( h,'linewidth',2 ); |
|
| 31 |
set( h,'linewidth',3 ); |
|
| 32 |
d = plot( Xs(j,idx),Ys(j,idx), '.k'); |
|
| 33 |
set( d,'MarkerSize',5 ); |
|
| 29 | 34 |
end |
| 30 | 35 |
hold off; |
| 31 | 36 |
|
| 32 |
if makeMovie == true |
|
| 33 |
if idx < 3 %so we don't see giant arrows |
|
| 34 |
clf |
|
| 35 |
end |
|
| 36 |
mov(:,idx) = getframe(f,winsize); |
|
| 37 |
else |
|
| 38 |
mov = []; |
|
| 37 |
if idx < 3 %s o we don't see giant arrows |
|
| 38 |
clf |
|
| 39 | 39 |
end |
| 40 |
mov(:,idx) = getframe(f,winsize); |
|
| 40 | 41 |
|
| 41 | 42 |
end |
Also available in: Unified diff