plot.html

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

0316 <span class="comment">% If current figure is empty, draw robot in it</span>
0317 <span class="comment">% If current figure has hold on, add robot to it</span>
0318 <span class="comment">% Otherwise, create new figure and draw robot in it.</span>
0319 <span class="comment">%</span>
0320 
0321 <a name="_sub2" href="#_subfunctions" class="code">function h = create_new_robot(robot, opt)</a>
0322     h.mag = opt.mag;
0323 
0324     <span class="comment">%</span>
0325     <span class="comment">% setup an axis in which to animate the robot</span>
0326     <span class="comment">%</span>
0327     <span class="comment">% handles not provided, create graphics</span>
0328     <span class="comment">%disp('in creat_new_robot')</span>
0329     <span class="keyword">if</span> ~ishold,
0330         <span class="comment">% if current figure has hold on, then draw robot here</span>
0331         <span class="comment">% otherwise, create a new figure</span>
0332         axis(opt.dims);
0333     <span class="keyword">end</span>
0334     xlabel(<span class="string">'X'</span>)
0335     ylabel(<span class="string">'Y'</span>)
0336     zlabel(<span class="string">'Z'</span>)
0337     set(gca, <span class="string">'drawmode'</span>, <span class="string">'fast'</span>);
0338     grid on
0339 
0340 
0341     zlim = get(gca, <span class="string">'ZLim'</span>);
0342     h.zmin = zlim(1);
0343 
0344     <span class="keyword">if</span> opt.base,
0345         b = transl(robot.base);
0346         line(<span class="string">'xdata'</span>, [b(1);b(1)], <span class="keyword">...</span>
0347             <span class="string">'ydata'</span>, [b(2);b(2)], <span class="keyword">...</span>
0348             <span class="string">'zdata'</span>, [h.zmin;b(3)], <span class="keyword">...</span>
0349             <span class="string">'LineWidth'</span>, 4, <span class="keyword">...</span>
0350             <span class="string">'color'</span>, <span class="string">'red'</span>);
0351     <span class="keyword">end</span>
0352     <span class="keyword">if</span> opt.name,
0353         b = transl(robot.base);
0354         text(b(1), b(2)-opt.mag, [<span class="string">' '</span> robot.name])
0355     <span class="keyword">end</span>
0356     <span class="comment">% create a line which we will</span>
0357     <span class="comment">% subsequently modify.  Set erase mode to xor for fast</span>
0358     <span class="comment">% update</span>
0359     <span class="comment">%</span>
0360     h.robot = line(robot.lineopt{:}, <span class="keyword">...</span>
0361         <span class="string">'Erasemode'</span>, opt.erasemode);
0362     <span class="keyword">if</span> opt.shadow == 1,
0363         h.shadow = line(robot.shadowopt{:}, <span class="keyword">...</span>
0364             <span class="string">'Erasemode'</span>, opt.erasemode);
0365     <span class="keyword">end</span>
0366 
0367     <span class="keyword">if</span> opt.wrist == 1,    
0368         h.x = line(<span class="string">'xdata'</span>, [0;0], <span class="keyword">...</span>
0369             <span class="string">'ydata'</span>, [0;0], <span class="keyword">...</span>
0370             <span class="string">'zdata'</span>, [0;0], <span class="keyword">...</span>
0371             <span class="string">'color'</span>, <span class="string">'red'</span>, <span class="keyword">...</span>
0372             <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0373         h.y = line(<span class="string">'xdata'</span>, [0;0], <span class="keyword">...</span>
0374             <span class="string">'ydata'</span>, [0;0], <span class="keyword">...</span>
0375             <span class="string">'zdata'</span>, [0;0], <span class="keyword">...</span>
0376             <span class="string">'color'</span>, <span class="string">'green'</span>, <span class="keyword">...</span>
0377             <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0378         h.z = line(<span class="string">'xdata'</span>, [0;0], <span class="keyword">...</span>
0379             <span class="string">'ydata'</span>, [0;0], <span class="keyword">...</span>
0380             <span class="string">'zdata'</span>, [0;0], <span class="keyword">...</span>
0381             <span class="string">'color'</span>, <span class="string">'blue'</span>, <span class="keyword">...</span>
0382             <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0383         h.xt = text(0, 0, opt.wristlabel(1), <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0384         h.yt = text(0, 0, opt.wristlabel(2), <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0385         h.zt = text(0, 0, opt.wristlabel(3), <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0386 
0387     <span class="keyword">end</span>
0388 
0389     <span class="comment">%</span>
0390     <span class="comment">% display cylinders (revolute) or boxes (pristmatic) at</span>
0391     <span class="comment">% each joint, as well as axis centerline.</span>
0392     <span class="comment">%</span>
0393     <span class="keyword">if</span> opt.joints == 1,
0394         L = robot.link;
0395         <span class="keyword">for</span> i=1:robot.n,
0396 
0397             <span class="comment">% cylinder or box to represent the joint</span>
0398             <span class="keyword">if</span> L{i}.sigma == 0,
0399                 N = 8;
0400             <span class="keyword">else</span>
0401                 N = 4;
0402             <span class="keyword">end</span>
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             <span class="comment">% add the surface object and color it</span>
0408             h.joint(i) = surface(xc,yc,zc);
0409             <span class="comment">%set(h.joint(i), 'erasemode', 'xor');</span>
0410             set(h.joint(i), <span class="string">'FaceColor'</span>, <span class="string">'blue'</span>);
0411 
0412             <span class="comment">% build a matrix of coordinates so we</span>
0413             <span class="comment">% can transform the cylinder in animate()</span>
0414             <span class="comment">% and hang it off the cylinder</span>
0415             xyz = [xc(:)'; yc(:)'; zc(:)'; ones(1,2*N+2)]; 
0416             set(h.joint(i), <span class="string">'UserData'</span>, xyz);
0417 
0418             <span class="comment">% add a dashed line along the axis</span>
0419             h.jointaxis(i) = line(<span class="string">'xdata'</span>, [0;0], <span class="keyword">...</span>
0420                 <span class="string">'ydata'</span>, [0;0], <span class="keyword">...</span>
0421                 <span class="string">'zdata'</span>, [0;0], <span class="keyword">...</span>
0422                 <span class="string">'color'</span>, <span class="string">'blue'</span>, <span class="keyword">...</span>
0423                 <span class="string">'linestyle'</span>, <span class="string">'--'</span>, <span class="keyword">...</span>
0424                 <span class="string">'erasemode'</span>, <span class="string">'xor'</span>);
0425         <span class="keyword">end</span>
0426     <span class="keyword">end</span>
0427 
0428 <span class="comment">%ANIMATE   move an existing graphical robot</span>
0429 <span class="comment">%</span>
0430 <span class="comment">%    animate(robot, q)</span>
0431 <span class="comment">%</span>
0432 <span class="comment">% Move the graphical robot to the pose specified by the joint coordinates q.</span>
0433 <span class="comment">% Graphics are defined by the handle structure robot.handle.</span>
0434 
0435 <a name="_sub3" href="#_subfunctions" class="code">function animate(robot, q)</a>
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     <span class="comment">% compute the link transforms, and record the origin of each frame</span>
0453     <span class="comment">% for the animation.</span>
0454     t = robot.base;
0455     Tn = t;
0456     <span class="keyword">for</span> 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     <span class="keyword">end</span>
0468     t = t *robot.tool;
0469 
0470     <span class="comment">%</span>
0471     <span class="comment">% draw the robot stick figure and the shadow</span>
0472     <span class="comment">%</span>
0473     set(h.robot,<span class="string">'xdata'</span>, x, <span class="string">'ydata'</span>, y, <span class="string">'zdata'</span>, z);
0474     <span class="keyword">if</span> isfield(h, <span class="string">'shadow'</span>),
0475         set(h.shadow,<span class="string">'xdata'</span>, xs, <span class="string">'ydata'</span>, ys, <span class="string">'zdata'</span>, zs);
0476     <span class="keyword">end</span>
0477     
0478 
0479     <span class="comment">%</span>
0480     <span class="comment">% display the joints as cylinders with rotation axes</span>
0481     <span class="comment">%</span>
0482     <span class="keyword">if</span> isfield(h, <span class="string">'joint'</span>),
0483         xyz_line = [0 0; 0 0; -2*mag 2*mag; 1 1];
0484 
0485         <span class="keyword">for</span> j=1:n,
0486             <span class="comment">% get coordinate data from the cylinder</span>
0487             xyz = get(h.joint(j), <span class="string">'UserData'</span>);
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), <span class="string">'Xdata'</span>, xc, <span class="string">'Ydata'</span>, yc, <span class="keyword">...</span>
0495                 <span class="string">'Zdata'</span>, zc);
0496 
0497             xyzl = Tn(:,:,j) * xyz_line;
0498             set(h.jointaxis(j), <span class="string">'Xdata'</span>, xyzl(1,:), <span class="keyword">...</span>
0499                 <span class="string">'Ydata'</span>, xyzl(2,:), <span class="keyword">...</span>
0500                 <span class="string">'Zdata'</span>, xyzl(3,:));
0501         <span class="keyword">end</span>
0502     <span class="keyword">end</span>
0503 
0504     <span class="comment">%</span>
0505     <span class="comment">% display the wrist axes and labels</span>
0506     <span class="comment">%</span>
0507     <span class="keyword">if</span> isfield(h, <span class="string">'x'</span>),
0508         <span class="comment">%</span>
0509         <span class="comment">% compute the wrist axes, based on final link transformation</span>
0510         <span class="comment">% plus the tool transformation.</span>
0511         <span class="comment">%</span>
0512         xv = t*[mag;0;0;1];
0513         yv = t*[0;mag;0;1];
0514         zv = t*[0;0;mag;1];
0515 
0516         <span class="comment">%</span>
0517         <span class="comment">% update the line segments, wrist axis and links</span>
0518         <span class="comment">%</span>
0519         set(h.x,<span class="string">'xdata'</span>,[t(1,4) xv(1)], <span class="string">'ydata'</span>, [t(2,4) xv(2)], <span class="keyword">...</span>
0520             <span class="string">'zdata'</span>, [t(3,4) xv(3)]);
0521         set(h.y,<span class="string">'xdata'</span>,[t(1,4) yv(1)], <span class="string">'ydata'</span>, [t(2,4) yv(2)], <span class="keyword">...</span>
0522              <span class="string">'zdata'</span>, [t(3,4) yv(3)]);
0523         set(h.z,<span class="string">'xdata'</span>,[t(1,4) zv(1)], <span class="string">'ydata'</span>, [t(2,4) zv(2)], <span class="keyword">...</span>
0524              <span class="string">'zdata'</span>, [t(3,4) zv(3)]);
0525         set(h.xt, <span class="string">'Position'</span>, xv(1:3));
0526         set(h.yt, <span class="string">'Position'</span>, yv(1:3));
0527         set(h.zt, <span class="string">'Position'</span>, zv(1:3));
0528     <span class="keyword">end</span>
0529     
0530     drawnow</pre></div>
<hr><address>Generated on Sun 15-Feb-2009 18:09:29 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>