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.
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