function  WP_show_trace(trace,scale)
%
SCALE = scale;

num_steps = length(trace);
x = [];
y = [];
theta = [];
dirx = [];
diry = [];
for s = 1:num_steps
    T = trace(s).T_actual;
    x = [x;T(1,4)];
    y = [y;T(2,4)];
    theta_s = atan2(T(2,1),T(1,1));
    theta = [theta;theta_s];
    dirx = [dirx;cos(theta_s)];
    diry = [diry;sin(theta_s)];
end
clf
hold on
for p = 1:num_steps
   plot(x(p),y(p),'bo');
   plot([x(p),x(p)+SCALE*dirx(p)],[y(p),y(p)+SCALE*diry(p)],'r');
end
