You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
55 lines
1.1 KiB
55 lines
1.1 KiB
%plot_vehicle Plot ground vehicle pose
|
|
%
|
|
% plot_vehicle(X,OPTIONS) draw representation of ground robot as an
|
|
% oriented triangle with pose X (1x3) [x,y,theta] or X (3x3) as homogeneous
|
|
% transform in SE(2).
|
|
%
|
|
% Options::
|
|
% 'scale',S Draw vehicle with length S x maximum axis dimension
|
|
% 'size',S Draw vehicle with length S
|
|
%
|
|
% See also Vehicle.plot.
|
|
|
|
% TODO needs to work for 3D point
|
|
|
|
function plot_vehicle(x, varargin)
|
|
|
|
opt.scale = 1/60;
|
|
opt.size = [];
|
|
|
|
[opt,args] = tb_optparse(opt, varargin);
|
|
|
|
% get the current axes
|
|
a = axis;
|
|
|
|
% compute the dimensions of the robot
|
|
if ~isempty(opt.size)
|
|
d = opt.size;
|
|
else
|
|
d = (a(2)+a(4) - a(1)-a(3)) * opt.scale;
|
|
end
|
|
|
|
if numel(x) == 3
|
|
% convert vector form of pose to SE(2)
|
|
T = se2(x(1), x(2), x(3));
|
|
else
|
|
T = x;
|
|
end
|
|
|
|
% draw it
|
|
% points = [
|
|
% d 0 1
|
|
% -d -0.6*d 1
|
|
% -d 0.6*d 1
|
|
% d 0 1]';
|
|
|
|
points = [
|
|
d 0
|
|
-d -0.6*d
|
|
-d 0.6*d]';
|
|
|
|
points = homtrans(T, points);
|
|
|
|
plot_poly(points, args{:});
|
|
|
|
end
|