Home > @robot > plot.m

plot

PURPOSE ^

PLOT Graphical robot animation

SYNOPSIS ^

function rnew = plot(robot, tg, varargin)

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SUBFUNCTIONS ^

SOURCE CODE ^

0001 %PLOT Graphical robot animation
0002 %
0003 %    PLOT(ROBOT, Q)
0004 %    PLOT(ROBOT, Q, options)
0005 %
0006 % Displays a graphical animation of a robot based on the
0007 % kinematic model.  A stick figure polyline joins the origins of
0008 % the link coordinate frames.
0009 % The robot is displayed at the joint angle Q, or if a matrix it is
0010 % animated as the robot moves along the trajectory.
0011 %
0012 % The graphical robot object holds a copy of the robot object and
0013 % the graphical element is tagged with the robot's name (.name method).
0014 % This state also holds the last joint configuration which can be retrieved,
0015 % see PLOT(robot) below.
0016 %
0017 % If no robot of this name is currently displayed then a robot will
0018 % be drawn in the current figure.  If hold is enabled (hold on) then the
0019 % robot will be added to the current figure.
0020 %
0021 % If the robot already exists then that graphical model will be found
0022 % and moved.
0023 %
0024 % MULTIPLE VIEWS OF THE SAME ROBOT
0025 % If one or more plots of this robot already exist then these will all
0026 % be moved according to the argument Q.  All robots in all windows with
0027 % the same name will be moved.
0028 %
0029 % MULTIPLE ROBOTS
0030 % Multiple robots can be displayed in the same plot, by using "hold on"
0031 % before calls to plot(robot).
0032 %
0033 % options is a list of any of the following:
0034 % 'workspace' [xmin, xmax ymin ymax zmin zmax]
0035 % 'perspective' 'ortho'        controls camera view mode
0036 % 'erase' 'noerase'        controls erasure of arm during animation
0037 % 'loop' 'noloop'        controls endless loop mode
0038 % 'base' 'nobase'        controls display of base 'pedestal'
0039 % 'wrist' 'nowrist'        controls display of wrist
0040 % 'name', 'noname'        display the robot's name
0041 % 'shadow' 'noshadow'        controls display of shadow
0042 % 'xyz' 'noa'            wrist axis label
0043 % 'joints' 'nojoints'        controls display of joints
0044 % 'mag' scale            annotation scale factor
0045 %
0046 % The options come from 3 sources and are processed in the order:
0047 % 1. Cell array of options returned by the function PLOTBOTOPT
0048 % 2. Cell array of options returned by the .plotopt method of the
0049 %    robot object.  These are set by the .plotopt method.
0050 % 3. List of arguments in the command line.
0051 %
0052 % GRAPHICAL ANNOTATIONS:
0053 %
0054 % The basic stick figure robot can be annotated with
0055 %  shadow on the floor
0056 %  XYZ wrist axes and labels
0057 %  joint cylinders and axes
0058 %
0059 % All of these require some kind of dimension and this is determined
0060 % using a simple heuristic from the workspace dimensions.  This dimension
0061 % can be changed by setting the multiplicative scale factor using the
0062 % 'mag' option above.
0063 %
0064 % GETTING GRAPHICAL ROBOT STATE:
0065 % q = PLOT(ROBOT)
0066 % Returns the joint configuration from the state of an existing
0067 % graphical representation of the specified robot.  If multiple
0068 % instances exist, that of the first one returned by findobj() is
0069 % given.
0070 %
0071 % MOVING JUST ONE INSTANCE oF A ROBOT:
0072 %
0073 %  r = PLOT(robot, q)
0074 %
0075 % Returns a copy of the robot object, with the .handle element set.
0076 %
0077 %  PLOT(r, q)
0078 %
0079 % will animate just this instance, not all robots of the same name.
0080 %
0081 % See also: PLOTBOTOPT, DRIVEBOT, FKINE, ROBOT.
0082 
0083 
0084 % HANDLES:
0085 %
0086 %  A robot comprises a bunch of individual graphical elements and these are
0087 % kept in a structure which can be stored within the .handle element of a
0088 % robot object:
0089 %    h.robot        the robot stick figure
0090 %    h.shadow    the robot's shadow
0091 %    h.x        wrist vectors
0092 %    h.y
0093 %    h.z
0094 %    h.xt        wrist vector labels
0095 %    h.yt
0096 %    h.zt
0097 %
0098 %  The plot function returns a new robot object with the handle element set.
0099 %
0100 % For the h.robot object we additionally:
0101 %    - save this new robot object as its UserData
0102 %    - tag it with the name field from the robot object
0103 %
0104 %  This enables us to find all robots with a given name, in all figures,
0105 % and update them.
0106 
0107 % Copyright (C) 1993-2008, by Peter I. Corke
0108 %
0109 % This file is part of The Robotics Toolbox for Matlab (RTB).
0110 %
0111 % RTB is free software: you can redistribute it and/or modify
0112 % it under the terms of the GNU Lesser General Public License as published by
0113 % the Free Software Foundation, either version 3 of the License, or
0114 % (at your option) any later version.
0115 %
0116 % RTB is distributed in the hope that it will be useful,
0117 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0118 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0119 % GNU Lesser General Public License for more details.
0120 %
0121 % You should have received a copy of the GNU Leser General Public License
0122 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0123 
0124 function rnew = plot(robot, tg, varargin)
0125 
0126     %
0127     % q = PLOT(robot)
0128     % return joint coordinates from a graphical robot of given name
0129     %
0130     if nargin == 1,
0131         rh = findobj('Tag', robot.name);
0132         if ~isempty(rh),
0133             r = get(rh(1), 'UserData');
0134             rnew = r.q;
0135         end
0136         return
0137     end
0138     
0139     % process options
0140     opt = plot_options(robot, varargin);
0141 
0142     %
0143     % robot2 = ROBOT(robot, q, varargin)
0144     %
0145     np = numrows(tg);
0146     n = robot.n;
0147 
0148     if numcols(tg) ~= n,
0149         error('Insufficient columns in q')
0150     end
0151 
0152     if ~isempty(robot.handle),
0153         %disp('has handles')
0154         % handles provided, animate just that robot
0155         for r=1:opt.repeat,
0156             for p=1:np,
0157             animate( robot, tg(p,:));
0158             pause(opt.delay)
0159             end
0160         end
0161 
0162         return;
0163     end
0164 
0165     % Do the right thing with figure windows.
0166     ax = newplot();
0167     
0168     % if this figure has no robot in it, create one
0169     if isempty( findobj(ax, 'Tag', robot.name) ),
0170 
0171         h = create_new_robot(robot, opt);
0172 
0173         % save the handles in the passed robot object, and
0174         % attach it to the robot as user data.
0175         robot.handle = h;
0176         set(h.robot, 'Tag', robot.name);
0177         set(h.robot, 'UserData', robot);
0178     end
0179 
0180     % get handle of any existing robot of same name
0181     rh = findobj('Tag', robot.name);
0182 
0183     % now animate all robots tagged with this name
0184 
0185     rh = findobj('Tag', robot.name);
0186     for r=1:opt.repeat,
0187         for p=1:np,
0188         for r=rh',
0189             animate( get(r, 'UserData'), tg(p,:));
0190         end
0191         end
0192     end
0193 
0194     % save the last joint angles away in the graphical robot
0195     for r=rh',
0196         rr = get(r, 'UserData');
0197         rr.q = tg(end,:);
0198         set(r, 'UserData', rr);
0199     end
0200 
0201     if nargout > 0,
0202         rnew = robot;
0203     end
0204 
0205 %PLOT_OPTIONS
0206 %
0207 %    o = PLOT_OPTIONS(robot, options)
0208 %
0209 % Returns an options structure
0210 
0211 function o = plot_options(robot, optin)
0212     %%%%%%%%%%%%%% process options
0213     o.erasemode = 'xor';
0214     o.joints = 1;
0215     o.wrist = 1;
0216     o.repeat = 1;
0217     o.shadow = 1;
0218     o.wrist = 1;
0219     o.dims = [];
0220     o.base = 0;
0221     o.wristlabel = 'xyz';
0222     o.projection = 'orthographic';
0223     o.magscale = 1;
0224     o.name = 1;
0225     o.delay = 0;
0226 
0227     % read options string in the order
0228     %    1. robot.plotopt
0229     %    2. the M-file plotbotopt if it exists
0230     %    3. command line arguments
0231     if exist('plotbotopt', 'file') == 2,
0232         options = plotbotopt;
0233         options = [options robot.plotopt optin];
0234     else
0235         options = [robot.plotopt optin];
0236     end
0237     i = 1;
0238     while i <= length(options),
0239         switch lower(options{i}),
0240         case 'workspace'
0241             o.dims = options{i+1};
0242             i = i+1;
0243         case 'mag'
0244             o.magscale = options{i+1};
0245             i = i+1;
0246         case 'perspective'
0247             o.projection = 'perspective';
0248         case 'ortho'
0249             o.projection = 'orthographic';
0250         case 'erase'
0251             o.erasemode = 'xor';
0252         case 'noerase'
0253             o.erasemode = 'none';
0254         case 'base'
0255             o.base = 1;
0256         case 'nobase'
0257             o.base = 0;
0258         case 'loop'
0259             o.repeat = Inf;
0260         case 'noloop'
0261             o.repeat = 1;
0262         case 'name',
0263             o.name = 1;
0264         case 'noname',
0265             o.name = 0;
0266         case 'wrist'
0267             o.wrist = 1;
0268         case 'nowrist'
0269             o.wrist = 0;
0270         case 'shadow'
0271             o.shadow = 1;
0272         case 'noshadow'
0273             o.shadow = 0;
0274         case 'xyz'
0275             o.wristlabel = 'XYZ';
0276         case 'noa'
0277             o.wristlabel = 'NOA';
0278         case 'joints'
0279             o.joints = 1;
0280         case 'nojoints'
0281             o.joints = 0;
0282         case 'delay'
0283             o.delay = options{i+1};
0284             i = i + 1;
0285         otherwise
0286             error(['unknown option: ' options{i}]);
0287         end
0288         i = i+1;
0289     end
0290 
0291     if isempty(o.dims),
0292         %
0293         % simple heuristic to figure the maximum reach of the robot
0294         %
0295         L = robot.link;
0296         reach = 0;
0297         for i=1:robot.n,
0298             reach = reach + abs(L{i}.A) + abs(L{i}.D);
0299         end
0300         o.dims = [-reach reach -reach reach -reach reach];
0301         o.mag = reach/10;
0302     else
0303         reach = min(abs(dims));
0304     end
0305     o.mag = o.magscale * reach/10;
0306 
0307 %CREATE_NEW_ROBOT
0308 %
0309 %    h = CREATE_NEW_ROBOT(robot, opt)
0310 %
0311 % Using data from robot object and options create a graphical robot in
0312 % the current figure.
0313 %
0314 % Returns a structure of handles to graphical objects.
0315 %
0316 % If current figure is empty, draw robot in it
0317 % If current figure has hold on, add robot to it
0318 % Otherwise, create new figure and draw robot in it.
0319 %
0320 
0321 function h = create_new_robot(robot, opt)
0322     h.mag = opt.mag;
0323 
0324     %
0325     % setup an axis in which to animate the robot
0326     %
0327     % handles not provided, create graphics
0328     %disp('in creat_new_robot')
0329     if ~ishold,
0330         % if current figure has hold on, then draw robot here
0331         % otherwise, create a new figure
0332         axis(opt.dims);
0333     end
0334     xlabel('X')
0335     ylabel('Y')
0336     zlabel('Z')
0337     set(gca, 'drawmode', 'fast');
0338     grid on
0339 
0340 
0341     zlim = get(gca, 'ZLim');
0342     h.zmin = zlim(1);
0343 
0344     if opt.base,
0345         b = transl(robot.base);
0346         line('xdata', [b(1);b(1)], ...
0347             'ydata', [b(2);b(2)], ...
0348             'zdata', [h.zmin;b(3)], ...
0349             'LineWidth', 4, ...
0350             'color', 'red');
0351     end
0352     if opt.name,
0353         b = transl(robot.base);
0354         text(b(1), b(2)-opt.mag, [' ' robot.name])
0355     end
0356     % create a line which we will
0357     % subsequently modify.  Set erase mode to xor for fast
0358     % update
0359     %
0360     h.robot = line(robot.lineopt{:}, ...
0361         'Erasemode', opt.erasemode);
0362     if opt.shadow == 1,
0363         h.shadow = line(robot.shadowopt{:}, ...
0364             'Erasemode', opt.erasemode);
0365     end
0366 
0367     if opt.wrist == 1,    
0368         h.x = line('xdata', [0;0], ...
0369             'ydata', [0;0], ...
0370             'zdata', [0;0], ...
0371             'color', 'red', ...
0372             'erasemode', 'xor');
0373         h.y = line('xdata', [0;0], ...
0374             'ydata', [0;0], ...
0375             'zdata', [0;0], ...
0376             'color', 'green', ...
0377             'erasemode', 'xor');
0378         h.z = line('xdata', [0;0], ...
0379             'ydata', [0;0], ...
0380             'zdata', [0;0], ...
0381             'color', 'blue', ...
0382             'erasemode', 'xor');
0383         h.xt = text(0, 0, opt.wristlabel(1), 'erasemode', 'xor');
0384         h.yt = text(0, 0, opt.wristlabel(2), 'erasemode', 'xor');
0385         h.zt = text(0, 0, opt.wristlabel(3), 'erasemode', 'xor');
0386 
0387     end
0388 
0389     %
0390     % display cylinders (revolute) or boxes (pristmatic) at
0391     % each joint, as well as axis centerline.
0392     %
0393     if opt.joints == 1,
0394         L = robot.link;
0395         for i=1:robot.n,
0396 
0397             % cylinder or box to represent the joint
0398             if L{i}.sigma == 0,
0399                 N = 8;
0400             else
0401                 N = 4;
0402             end
0403             [xc,yc,zc] = cylinder(opt.mag/4, N);
0404             zc(zc==0) = -opt.mag/2;
0405             zc(zc==1) = opt.mag/2;
0406 
0407             % add the surface object and color it
0408             h.joint(i) = surface(xc,yc,zc);
0409             %set(h.joint(i), 'erasemode', 'xor');
0410             set(h.joint(i), 'FaceColor', 'blue');
0411 
0412             % build a matrix of coordinates so we
0413             % can transform the cylinder in animate()
0414             % and hang it off the cylinder
0415             xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; 
0416             set(h.joint(i), 'UserData', xyz);
0417 
0418             % add a dashed line along the axis
0419             h.jointaxis(i) = line('xdata', [0;0], ...
0420                 'ydata', [0;0], ...
0421                 'zdata', [0;0], ...
0422                 'color', 'blue', ...
0423                 'linestyle', '--', ...
0424                 'erasemode', 'xor');
0425         end
0426     end
0427 
0428 %ANIMATE   move an existing graphical robot
0429 %
0430 %    animate(robot, q)
0431 %
0432 % Move the graphical robot to the pose specified by the joint coordinates q.
0433 % Graphics are defined by the handle structure robot.handle.
0434 
0435 function animate(robot, q)
0436 
0437     n = robot.n;
0438     h = robot.handle;
0439     L = robot.link;
0440 
0441     mag = h.mag;
0442 
0443     b = transl(robot.base);
0444     x = b(1);
0445     y = b(2);
0446     z = b(3);
0447 
0448     xs = b(1);
0449     ys = b(2);
0450     zs = h.zmin;
0451 
0452     % compute the link transforms, and record the origin of each frame
0453     % for the animation.
0454     t = robot.base;
0455     Tn = t;
0456     for j=1:n,
0457         Tn(:,:,j) = t;
0458 
0459         t = t * L{j}(q(j));
0460 
0461         x = [x; t(1,4)];
0462         y = [y; t(2,4)];
0463         z = [z; t(3,4)];
0464         xs = [xs; t(1,4)];
0465         ys = [ys; t(2,4)];
0466         zs = [zs; h.zmin];
0467     end
0468     t = t *robot.tool;
0469 
0470     %
0471     % draw the robot stick figure and the shadow
0472     %
0473     set(h.robot,'xdata', x, 'ydata', y, 'zdata', z);
0474     if isfield(h, 'shadow'),
0475         set(h.shadow,'xdata', xs, 'ydata', ys, 'zdata', zs);
0476     end
0477     
0478 
0479     %
0480     % display the joints as cylinders with rotation axes
0481     %
0482     if isfield(h, 'joint'),
0483         xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1];
0484 
0485         for j=1:n,
0486             % get coordinate data from the cylinder
0487             xyz = get(h.joint(j), 'UserData');
0488             xyz = Tn(:,:,j) * xyz;
0489             ncols = numcols(xyz)/2;
0490             xc = reshape(xyz(1,:), 2, ncols);
0491             yc = reshape(xyz(2,:), 2, ncols);
0492             zc = reshape(xyz(3,:), 2, ncols);
0493 
0494             set(h.joint(j), 'Xdata', xc, 'Ydata', yc, ...
0495                 'Zdata', zc);
0496 
0497             xyzl = Tn(:,:,j) * xyz_line;
0498             set(h.jointaxis(j), 'Xdata', xyzl(1,:), ...
0499                 'Ydata', xyzl(2,:), ...
0500                 'Zdata', xyzl(3,:));
0501         end
0502     end
0503 
0504     %
0505     % display the wrist axes and labels
0506     %
0507     if isfield(h, 'x'),
0508         %
0509         % compute the wrist axes, based on final link transformation
0510         % plus the tool transformation.
0511         %
0512         xv = t*[mag;0;0;1];
0513         yv = t*[0;mag;0;1];
0514         zv = t*[0;0;mag;1];
0515 
0516         %
0517         % update the line segments, wrist axis and links
0518         %
0519         set(h.x,'xdata',[t(1,4) xv(1)], 'ydata', [t(2,4) xv(2)], ...
0520             'zdata', [t(3,4) xv(3)]);
0521         set(h.y,'xdata',[t(1,4) yv(1)], 'ydata', [t(2,4) yv(2)], ...
0522              'zdata', [t(3,4) yv(3)]);
0523         set(h.z,'xdata',[t(1,4) zv(1)], 'ydata', [t(2,4) zv(2)], ...
0524              'zdata', [t(3,4) zv(3)]);
0525         set(h.xt, 'Position', xv(1:3));
0526         set(h.yt, 'Position', yv(1:3));
0527         set(h.zt, 'Position', zv(1:3));
0528     end
0529     
0530     drawnow

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003