plot.html

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

0096 <span class="comment">%    h.zt</span>
0097 <span class="comment">%</span>
0098 <span class="comment">%  The plot function returns a new robot object with the handle element set.</span>
0099 <span class="comment">%</span>
0100 <span class="comment">% For the h.robot object we additionally:</span>
0101 <span class="comment">%    - save this new robot object as its UserData</span>
0102 <span class="comment">%    - tag it with the name field from the robot object</span>
0103 <span class="comment">%</span>
0104 <span class="comment">%  This enables us to find all robots with a given name, in all figures,</span>
0105 <span class="comment">% and update them.</span>
0106 
0107 <span class="comment">% Copyright (C) 1993-2008, by Peter I. Corke</span>
0108 <span class="comment">%</span>
0109 <span class="comment">% This file is part of The Robotics Toolbox for Matlab (RTB).</span>
0110 <span class="comment">%</span>
0111 <span class="comment">% RTB is free software: you can redistribute it and/or modify</span>
0112 <span class="comment">% it under the terms of the GNU Lesser General Public License as published by</span>
0113 <span class="comment">% the Free Software Foundation, either version 3 of the License, or</span>
0114 <span class="comment">% (at your option) any later version.</span>
0115 <span class="comment">%</span>
0116 <span class="comment">% RTB is distributed in the hope that it will be useful,</span>
0117 <span class="comment">% but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
0118 <span class="comment">% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
0119 <span class="comment">% GNU Lesser General Public License for more details.</span>
0120 <span class="comment">%</span>
0121 <span class="comment">% You should have received a copy of the GNU Leser General Public License</span>
0122 <span class="comment">% along with RTB.  If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
0123 
0124 <a name="_sub0" href="#_subfunctions" class="code">function rnew = plot(robot, tg, varargin)</a>
0125 
0126     <span class="comment">%</span>
0127     <span class="comment">% q = PLOT(robot)</span>
0128     <span class="comment">% return joint coordinates from a graphical robot of given name</span>
0129     <span class="comment">%</span>
0130     <span class="keyword">if</span> nargin == 1,
0131         rh = findobj(<span class="string">'Tag'</span>, robot.name);
0132         <span class="keyword">if</span> ~isempty(rh),
0133             r = get(rh(1), <span class="string">'UserData'</span>);
0134             rnew = r.q;
0135         <span class="keyword">end</span>
0136         <span class="keyword">return</span>
0137     <span class="keyword">end</span>
0138     
0139     <span class="comment">% process options</span>
0140     opt = <a href="#_sub1" class="code" title="subfunction o = plot_options(robot, optin)">plot_options</a>(<a href="robot.html" class="code" title="function r = robot(L, a1, a2, a3)">robot</a>, varargin);
0141 
0142     <span class="comment">%</span>
0143     <span class="comment">% robot2 = ROBOT(robot, q, varargin)</span>
0144     <span class="comment">%</span>
0145     np = numrows(tg);
0146     n = robot.n;
0147 
0148     <span class="keyword">if</span> numcols(tg) ~= n,
0149         error(<span class="string">'Insufficient columns in q'</span>)
0150     <span class="keyword">end</span>
0151 
0152     <span class="keyword">if</span> ~isempty(robot.handle),
0153         <span class="comment">%disp('has handles')</span>
0154         <span class="comment">% handles provided, animate just that robot</span>
0155         <span class="keyword">for</span> r=1:opt.repeat,
0156             <span class="keyword">for</span> p=1:np,
0157             <a href="#_sub3" class="code" title="subfunction animate(robot, q)">animate</a>( <a href="robot.html" class="code" title="function r = robot(L, a1, a2, a3)">robot</a>, tg(p,:));
0158             pause(opt.delay)
0159             <span class="keyword">end</span>
0160         <span class="keyword">end</span>
0161 
0162         <span class="keyword">return</span>;
0163     <span class="keyword">end</span>
0164 
0165     <span class="comment">% Do the right thing with figure windows.</span>
0166     ax = newplot();
0167     
0168     <span class="comment">% if this figure has no robot in it, create one</span>
0169     <span class="keyword">if</span> isempty( findobj(ax, <span class="string">'Tag'</span>, robot.name) ),
0170 
0171         h = <a href="#_sub2" class="code" title="subfunction h = create_new_robot(robot, opt)">create_new_robot</a>(<a href="robot.html" class="code" title="function r = robot(L, a1, a2, a3)">robot</a>, opt);
0172 
0173         <span class="comment">% save the handles in the passed robot object, and</span>
0174         <span class="comment">% attach it to the robot as user data.</span>
0175         robot.handle = h;
0176         set(h.robot, <span class="string">'Tag'</span>, robot.name);
0177         set(h.robot, <span class="string">'UserData'</span>, <a href="robot.html" class="code" title="function r = robot(L, a1, a2, a3)">robot</a>);
0178     <span class="keyword">end</span>
0179 
0180     <span class="comment">% get handle of any existing robot of same name</span>
0181     rh = findobj(<span class="string">'Tag'</span>, robot.name);
0182 
0183     <span class="comment">% now animate all robots tagged with this name</span>
0184 
0185     rh = findobj(<span class="string">'Tag'</span>, robot.name);
0186     <span class="keyword">for</span> r=1:opt.repeat,
0187         <span class="keyword">for</span> p=1:np,
0188         <span class="keyword">for</span> r=rh',
0189             <a href="#_sub3" class="code" title="subfunction animate(robot, q)">animate</a>( get(r, <span class="string">'UserData'</span>), tg(p,:));
0190         <span class="keyword">end</span>
0191         <span class="keyword">end</span>
0192     <span class="keyword">end</span>
0193 
0194     <span class="comment">% save the last joint angles away in the graphical robot</span>
0195     <span class="keyword">for</span> r=rh',
0196         rr = get(r, <span class="string">'UserData'</span>);
0197         rr.q = tg(<span class="keyword">end</span>,:);
0198         set(r, <span class="string">'UserData'</span>, rr);
0199     <span class="keyword">end</span>
0200 
0201     <span class="keyword">if</span> nargout &gt; 0,
0202         rnew = <a href="robot.html" class="code" title="function r = robot(L, a1, a2, a3)">robot</a>;
0203     <span class="keyword">end</span>
0204 
0205 <span class="comment">%PLOT_OPTIONS</span>
0206 <span class="comment">%</span>
0207 <span class="comment">%    o = PLOT_OPTIONS(robot, options)</span>
0208 <span class="comment">%</span>
0209 <span class="comment">% Returns an options structure</span>
0210 
0211 <a name="_sub1" href="#_subfunctions" class="code">function o = plot_options(robot, optin)</a>
0212     <span class="comment">%%%%%%%%%%%%%% process options</span>
0213     o.erasemode = <span class="string">'xor'</span>;
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 = <span class="string">'xyz'</span>;
0222     o.projection = <span class="string">'orthographic'</span>;
0223     o.magscale = 1;
0224     o.name = 1;
0225     o.delay = 0;
0226 
0227     <span class="comment">% read options string in the order</span>
0228     <span class="comment">%    1. robot.plotopt</span>
0229     <span class="comment">%    2. the M-file plotbotopt if it exists</span>
0230     <span class="comment">%    3. command line arguments</span>
0231     <span class="keyword">if</span> exist(<span class="string">'plotbotopt'</span>, <span class="string">'file'</span>) == 2,
0232         options = plotbotopt;
0233         options = [options robot.plotopt optin];
0234     <span class="keyword">else</span>
0235         options = [robot.plotopt optin];
0236     <span class="keyword">end</span>
0237     i = 1;
0238     <span class="keyword">while</span> i &lt;= length(options),
0239         <span class="keyword">switch</span> lower(options{i}),
0240         <span class="keyword">case</span> <span class="string">'workspace'</span>
0241             o.dims = options{i+1};
0242             i = i+1;
0243         <span class="keyword">case</span> <span class="string">'mag'</span>
0244             o.magscale = options{i+1};
0245             i = i+1;
0246         <span class="keyword">case</span> <span class="string">'perspective'</span>
0247             o.projection = <span class="string">'perspective'</span>;
0248         <span class="keyword">case</span> <span class="string">'ortho'</span>
0249             o.projection = <span class="string">'orthographic'</span>;
0250         <span class="keyword">case</span> <span class="string">'erase'</span>
0251             o.erasemode = <span class="string">'xor'</span>;
0252         <span class="keyword">case</span> <span class="string">'noerase'</span>
0253             o.erasemode = <span class="string">'none'</span>;
0254         <span class="keyword">case</span> <span class="string">'base'</span>
0255             o.base = 1;
0256         <span class="keyword">case</span> <span class="string">'nobase'</span>
0257             o.base = 0;
0258         <span class="keyword">case</span> <span class="string">'loop'</span>
0259             o.repeat = Inf;
0260         <span class="keyword">case</span> <span class="string">'noloop'</span>
0261             o.repeat = 1;
0262         <span class="keyword">case</span> <span class="string">'name'</span>,
0263             o.name = 1;
0264         <span class="keyword">case</span> <span class="string">'noname'</span>,
0265             o.name = 0;
0266         <span class="keyword">case</span> <span class="string">'wrist'</span>
0267             o.wrist = 1;
0268         <span class="keyword">case</span> <span class="string">'nowrist'</span>
0269             o.wrist = 0;
0270         <span class="keyword">case</span> <span class="string">'shadow'</span>
0271             o.shadow = 1;
0272         <span class="keyword">case</span> <span class="string">'noshadow'</span>
0273             o.shadow = 0;
0274         <span class="keyword">case</span> <span class="string">'xyz'</span>
0275             o.wristlabel = <span class="string">'XYZ'</span>;
0276         <span class="keyword">case</span> <span class="string">'noa'</span>
0277             o.wristlabel = <span class="string">'NOA'</span>;
0278         <span class="keyword">case</span> <span class="string">'joints'</span>
0279             o.joints = 1;
0280         <span class="keyword">case</span> <span class="string">'nojoints'</span>
0281             o.joints = 0;
0282         <span class="keyword">case</span> <span class="string">'delay'</span>
0283             o.delay = options{i+1};
0284             i = i + 1;
0285         <span class="keyword">otherwise</span>
0286             error([<span class="string">'unknown option: '</span> options{i}]);
0287         <span class="keyword">end</span>
0288         i = i+1;
0289     <span class="keyword">end</span>
0290 
0291     <span class="keyword">if</span> isempty(o.dims),
0292         <span class="comment">%</span>
0293         <span class="comment">% simple heuristic to figure the maximum reach of the robot</span>
0294         <span class="comment">%</span>
0295         L = robot.link;
0296         reach = 0;
0297         <span class="keyword">for</span> i=1:robot.n,
0298             reach = reach + abs(L{i}.A) + abs(L{i}.D);
0299         <span class="keyword">end</span>
0300         o.dims = [-reach reach -reach reach -reach reach];
0301         o.mag = reach/10;
0302     <span class="keyword">else</span>
0303         reach = min(abs(dims));
0304     <span class="keyword">end</span>
0305     o.mag = o.magscale * reach/10;
0306 
0307 <span class="comment">%CREATE_NEW_ROBOT</span>
0308 <span class="comment">%</span>
0309 <span class="comment">%    h = CREATE_NEW_ROBOT(robot, opt)</span>
0310 <span class="comment">%</span>
0311 <span class="comment">% Using data from robot object and options create a graphical robot in</span>
0312 <span class="comment">% the current figure.</span>
0313 <span class="comment">%</span>
0314 <span class="comment">% Returns a structure of handles to graphical objects.</span>
0315 <span class="comment">%</span>