plot.m

Robot tool box - provides many functions that are useful in robotics including such things as kinem

%PLOT Graphical robot animation
%
%	PLOT(ROBOT, Q)
%	PLOT(ROBOT, Q, options)
%
% Displays a graphical animation of a robot based on the
% kinematic model.  A stick figure polyline joins the origins of
% the link coordinate frames.
% The robot is displayed at the joint angle Q, or if a matrix it is
% animated as the robot moves along the trajectory.
%
% The graphical robot object holds a copy of the robot object and
% the graphical element is tagged with the robot's name (.name method).
% This state also holds the last joint configuration which can be retrieved,
% see PLOT(robot) below.
%
% If no robot of this name is currently displayed then a robot will
% be drawn in the current figure.  If hold is enabled (hold on) then the
% robot will be added to the current figure.
%
% If the robot already exists then that graphical model will be found 
% and moved.
%
% MULTIPLE VIEWS OF THE SAME ROBOT
% If one or more plots of this robot already exist then these will all
% be moved according to the argument Q.  All robots in all windows with 
% the same name will be moved.
%
% MULTIPLE ROBOTS
% Multiple robots can be displayed in the same plot, by using "hold on"
% before calls to plot(robot).  
%
% options is a list of any of the following:
% 'workspace' [xmin, xmax ymin ymax zmin zmax]
% 'perspective' 'ortho'		controls camera view mode
% 'erase' 'noerase'		controls erasure of arm during animation
% 'loop' 'noloop'		controls endless loop mode
% 'base' 'nobase'		controls display of base 'pedestal'
% 'wrist' 'nowrist'		controls display of wrist
% 'name', 'noname'		display the robot's name 
% 'shadow' 'noshadow'		controls display of shadow
% 'xyz' 'noa'			wrist axis label
% 'joints' 'nojoints'		controls display of joints
% 'mag' scale			annotation scale factor
%
% The options come from 3 sources and are processed in the order:
% 1. Cell array of options returned by the function PLOTBOTOPT
% 2. Cell array of options returned by the .plotopt method of the
%    robot object.  These are set by the .plotopt method.
% 3. List of arguments in the command line.
%
% GRAPHICAL ANNOTATIONS:
%
% The basic stick figure robot can be annotated with
%  shadow on the floor
%  XYZ wrist axes and labels
%  joint cylinders and axes
%
% All of these require some kind of dimension and this is determined
% using a simple heuristic from the workspace dimensions.  This dimension
% can be changed by setting the multiplicative scale factor using the
% 'mag' option above.
%
% GETTING GRAPHICAL ROBOT STATE:
% q = PLOT(ROBOT)
% Returns the joint configuration from the state of an existing 
% graphical representation of the specified robot.  If multiple
% instances exist, that of the first one returned by findobj() is
% given.
%
% MOVING JUST ONE INSTANCE oF A ROBOT:
%
%  r = PLOT(robot, q)
%
% Returns a copy of the robot object, with the .handle element set.
%
%  PLOT(r, q)
%
% will animate just this instance, not all robots of the same name.
%
% See also: PLOTBOTOPT, DRIVEBOT, FKINE, ROBOT.


% HANDLES:
%
%  A robot comprises a bunch of individual graphical elements and these are 
% kept in a structure which can be stored within the .handle element of a
% robot object:
%	h.robot		the robot stick figure
%	h.shadow	the robot's shadow
%	h.x		wrist vectors
%	h.y
%	h.z
%	h.xt		wrist vector labels
%	h.yt
%	h.zt
%
%  The plot function returns a new robot object with the handle element set.
%
% For the h.robot object we additionally: 
%	- save this new robot object as its UserData
%	- tag it with the name field from the robot object
%
%  This enables us to find all robots with a given name, in all figures,
% and update them.

% Copyright (C) 1993-2008, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
% 
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.

function rnew = plot(robot, tg, varargin)

	%
	% q = PLOT(robot)
	% return joint coordinates from a graphical robot of given name
	%
	if nargin == 1,
		rh = findobj('Tag', robot.name);
		if ~isempty(rh),
			r = get(rh(1), 'UserData');
			rnew = r.q;
		end
		return
    end
    
    % process options
	opt = plot_options(robot, varargin);

	%
	% robot2 = ROBOT(robot, q, varargin)
	%
	np = numrows(tg);
	n = robot.n;

	if numcols(tg) ~= n,
		error('Insufficient columns in q')
	end

	if ~isempty(robot.handle),
        %disp('has handles')
		% handles provided, animate just that robot
		for r=1:opt.repeat,
		    for p=1:np,
			animate( robot, tg(p,:));
            pause(opt.delay)
		    end
		end

		return;
    end

	% Do the right thing with figure windows.
    ax = newplot();
    
    % if this figure has no robot in it, create one
    if isempty( findobj(ax, 'Tag', robot.name) ),

		h = create_new_robot(robot, opt);

		% save the handles in the passed robot object, and
		% attach it to the robot as user data.
		robot.handle = h;
		set(h.robot, 'Tag', robot.name);
		set(h.robot, 'UserData', robot);
    end

    % get handle of any existing robot of same name
	rh = findobj('Tag', robot.name);

	% now animate all robots tagged with this name

	rh = findobj('Tag', robot.name);
	for r=1:opt.repeat,
	    for p=1:np,
		for r=rh',
			animate( get(r, 'UserData'), tg(p,:));
		end
	    end
	end

	% save the last joint angles away in the graphical robot
	for r=rh',
		rr = get(r, 'UserData');
		rr.q = tg(end,:);
		set(r, 'UserData', rr);
	end

	if nargout > 0,
		rnew = robot;
	end

%PLOT_OPTIONS
%
%	o = PLOT_OPTIONS(robot, options)
%
% Returns an options structure

function o = plot_options(robot, optin)
	%%%%%%%%%%%%%% process options
	o.erasemode = 'xor';
	o.joints = 1;
	o.wrist = 1;
	o.repeat = 1;
	o.shadow = 1;
	o.wrist = 1;
	o.dims = [];
	o.base = 0;
	o.wristlabel = 'xyz';
	o.projection = 'orthographic';
	o.magscale = 1;
	o.name = 1;
    o.delay = 0;

	% read options string in the order
	%	1. robot.plotopt
	%	2. the M-file plotbotopt if it exists
	%	3. command line arguments
	if exist('plotbotopt', 'file') == 2,
		options = plotbotopt;
		options = [options robot.plotopt optin];
	else
		options = [robot.plotopt optin];
	end
	i = 1;
	while i <= length(options),
		switch lower(options{i}),
		case 'workspace'
			o.dims = options{i+1};
			i = i+1;
		case 'mag'
			o.magscale = options{i+1};
			i = i+1;
		case 'perspective'
			o.projection = 'perspective';
		case 'ortho'
			o.projection = 'orthographic';
		case 'erase'
			o.erasemode = 'xor';
		case 'noerase'
			o.erasemode = 'none';
		case 'base'
			o.base = 1;
		case 'nobase'
			o.base = 0;
		case 'loop'
			o.repeat = Inf;
		case 'noloop'
			o.repeat = 1;
		case 'name',
			o.name = 1;
		case 'noname',
			o.name = 0;
		case 'wrist'
			o.wrist = 1;
		case 'nowrist'
			o.wrist = 0;
		case 'shadow'
			o.shadow = 1;
		case 'noshadow'
			o.shadow = 0;
		case 'xyz'
			o.wristlabel = 'XYZ';
		case 'noa'
			o.wristlabel = 'NOA';
		case 'joints'
			o.joints = 1;
		case 'nojoints'
			o.joints = 0;
        case 'delay'
            o.delay = options{i+1};
            i = i + 1;
		otherwise
			error(['unknown option: ' options{i}]);
		end
		i = i+1;
	end

	if isempty(o.dims),
		%
		% simple heuristic to figure the maximum reach of the robot
		%
		L = robot.link;
		reach = 0;
		for i=1:robot.n,
			reach = reach + abs(L{i}.A) + abs(L{i}.D);
		end
		o.dims = [-reach reach -reach reach -reach reach];
		o.mag = reach/10;
	else
		reach = min(abs(dims));
	end
	o.mag = o.magscale * reach/10;

%CREATE_NEW_ROBOT
% 
%	h = CREATE_NEW_ROBOT(robot, opt)
%
% Using data from robot object and options create a graphical robot in
% the current figure.
%
% Returns a structure of handles to graphical objects.
%
% If current figure is empty, draw robot in it
% If current figure has hold on, add robot to it
% Otherwise, create new figure and draw robot in it.
%	

function h = create_new_robot(robot, opt)
	h.mag = opt.mag;

	%
	% setup an axis in which to animate the robot
	%
	% handles not provided, create graphics
	%disp('in creat_new_robot')
	if ~ishold,
		% if current figure has hold on, then draw robot here
		% otherwise, create a new figure
		axis(opt.dims);
	end
	xlabel('X')
	ylabel('Y')
	zlabel('Z')
	set(gca, 'drawmode', 'fast');
	grid on


	zlim = get(gca, 'ZLim');
	h.zmin = zlim(1);

	if opt.base,
		b = transl(robot.base);
		line('xdata', [b(1);b(1)], ...
			'ydata', [b(2);b(2)], ...
			'zdata', [h.zmin;b(3)], ...
			'LineWidth', 4, ...
			'color', 'red');
	end
	if opt.name,
		b = transl(robot.base);
		text(b(1), b(2)-opt.mag, [' ' robot.name])
	end
	% create a line which we will
	% subsequently modify.  Set erase mode to xor for fast
	% update
	%
	h.robot = line(robot.lineopt{:}, ...
		'Erasemode', opt.erasemode);
	if opt.shadow == 1,
		h.shadow = line(robot.shadowopt{:}, ...
			'Erasemode', opt.erasemode);
	end

	if opt.wrist == 1,	
		h.x = line('xdata', [0;0], ...
			'ydata', [0;0], ...
			'zdata', [0;0], ...
			'color', 'red', ...
			'erasemode', 'xor');
		h.y = line('xdata', [0;0], ...
			'ydata', [0;0], ...
			'zdata', [0;0], ...
			'color', 'green', ...
			'erasemode', 'xor');
		h.z = line('xdata', [0;0], ...
			'ydata', [0;0], ...
			'zdata', [0;0], ...
			'color', 'blue', ...
			'erasemode', 'xor');
		h.xt = text(0, 0, opt.wristlabel(1), 'erasemode', 'xor');
		h.yt = text(0, 0, opt.wristlabel(2), 'erasemode', 'xor');
		h.zt = text(0, 0, opt.wristlabel(3), 'erasemode', 'xor');

	end

	%
	% display cylinders (revolute) or boxes (pristmatic) at
	% each joint, as well as axis centerline.
	%
	if opt.joints == 1,
		L = robot.link;
		for i=1:robot.n,

			% cylinder or box to represent the joint
			if L{i}.sigma == 0,
				N = 8;
			else
				N = 4;
			end
			[xc,yc,zc] = cylinder(opt.mag/4, N);
			zc(zc==0) = -opt.mag/2;
			zc(zc==1) = opt.mag/2;

			% add the surface object and color it
			h.joint(i) = surface(xc,yc,zc);
			%set(h.joint(i), 'erasemode', 'xor');
			set(h.joint(i), 'FaceColor', 'blue');

			% build a matrix of coordinates so we
			% can transform the cylinder in animate()
			% and hang it off the cylinder
			xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; 
			set(h.joint(i), 'UserData', xyz);

			% add a dashed line along the axis
			h.jointaxis(i) = line('xdata', [0;0], ...
				'ydata', [0;0], ...
				'zdata', [0;0], ...
				'color', 'blue', ...
				'linestyle', '--', ...
				'erasemode', 'xor');
		end
	end

%ANIMATE   move an existing graphical robot
%
%	animate(robot, q)
%
% Move the graphical robot to the pose specified by the joint coordinates q.
% Graphics are defined by the handle structure robot.handle.

function animate(robot, q)

	n = robot.n;
	h = robot.handle;
	L = robot.link;

	mag = h.mag;

	b = transl(robot.base);
	x = b(1);
	y = b(2);
	z = b(3);

	xs = b(1);
	ys = b(2);
	zs = h.zmin;

	% compute the link transforms, and record the origin of each frame
	% for the animation.
	t = robot.base;
	Tn = t;
	for j=1:n,
		Tn(:,:,j) = t;

		t = t * L{j}(q(j));

		x = [x; t(1,4)];
		y = [y; t(2,4)];
		z = [z; t(3,4)];
		xs = [xs; t(1,4)];
		ys = [ys; t(2,4)];
		zs = [zs; h.zmin];
	end
	t = t *robot.tool;

	%
	% draw the robot stick figure and the shadow
	%
	set(h.robot,'xdata', x, 'ydata', y, 'zdata', z);
	if isfield(h, 'shadow'),
		set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs);
	end
	

	%
	% display the joints as cylinders with rotation axes
	%
	if isfield(h, 'joint'),
		xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1];

		for j=1:n,
			% get coordinate data from the cylinder
			xyz = get(h.joint(j), 'UserData');
			xyz = Tn(:,:,j) * xyz;
			ncols = numcols(xyz)/2;
			xc = reshape(xyz(1,:), 2, ncols);
			yc = reshape(xyz(2,:), 2, ncols);
			zc = reshape(xyz(3,:), 2, ncols);

			set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ...
				'Zdata', zc);

			xyzl = Tn(:,:,j) * xyz_line;
			set(h.jointaxis(j), 'Xdata', xyzl(1,:), ...
				'Ydata', xyzl(2,:), ...
				'Zdata', xyzl(3,:));
		end
	end

	%
	% display the wrist axes and labels
	%
	if isfield(h, 'x'),
		%
		% compute the wrist axes, based on final link transformation
		% plus the tool transformation.
		%
		xv = t*[mag;0;0;1];
		yv = t*[0;mag;0;1];
		zv = t*[0;0;mag;1];

		%
		% update the line segments, wrist axis and links
		%
		set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ...
			'zdata', [t(3,4) xv(3)]);
		set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ...
			 'zdata', [t(3,4) yv(3)]);
		set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ...
			 'zdata', [t(3,4) zv(3)]);
		set(h.xt, 'Position', xv(1:3));
		set(h.yt, 'Position', yv(1:3));
		set(h.zt, 'Position', zv(1:3));
	end
	
	drawnow