Home > . > drivebot.m

drivebot

PURPOSE ^

DRIVEBOT Drive a graphical robot

SYNOPSIS ^

function drivebot(a,b)

DESCRIPTION ^

DRIVEBOT Drive a graphical robot

   DRIVEBOT(ROBOT)
   DRIVEBOT(ROBOT, Q)

 Drive a graphical robot by means of a slider panel.

 If no graphical robot exists one is created in a new window.  Otherwise
 all graphical robots are 'driven'.

 If Q is specified it is used as the initial joint angle, otherwise
 it is taken from one of the existing graphical robots.

 SEE ALSO: ROBOT/PLOT, ROBOT

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %DRIVEBOT Drive a graphical robot
0002 %
0003 %   DRIVEBOT(ROBOT)
0004 %   DRIVEBOT(ROBOT, Q)
0005 %
0006 % Drive a graphical robot by means of a slider panel.
0007 %
0008 % If no graphical robot exists one is created in a new window.  Otherwise
0009 % all graphical robots are 'driven'.
0010 %
0011 % If Q is specified it is used as the initial joint angle, otherwise
0012 % it is taken from one of the existing graphical robots.
0013 %
0014 % SEE ALSO: ROBOT/PLOT, ROBOT
0015 
0016 % Copyright (C) 2001-2008, by Peter I. Corke
0017 %
0018 % This file is part of The Robotics Toolbox for Matlab (RTB).
0019 %
0020 % RTB is free software: you can redistribute it and/or modify
0021 % it under the terms of the GNU Lesser General Public License as published by
0022 % the Free Software Foundation, either version 3 of the License, or
0023 % (at your option) any later version.
0024 %
0025 % RTB is distributed in the hope that it will be useful,
0026 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0027 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0028 % GNU Lesser General Public License for more details.
0029 %
0030 % You should have received a copy of the GNU Leser General Public License
0031 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0032 
0033 function drivebot(a,b)
0034     bgcol = [135 206 250]/255;
0035 
0036     if isstr(a)
0037         % drivebot(name, j), graphical callback function
0038         name = a; % name of the robot
0039         j = b;  % joint index
0040         %disp(name)
0041         rh = findobj('Tag', name);
0042         %disp(rh)
0043         handles = get(gco, 'Userdata');
0044         scale = handles{3};
0045         for r=rh',
0046             rr = get(r, 'UserData');
0047             q = rr.q;
0048             if isempty(q),
0049                 q = zeros(1,rr.n);
0050             end
0051             if gco == handles{1},
0052                 % get value from slider
0053                 q(j) = get(gco, 'Value') / scale(j);
0054                 set(handles{2}, 'String', num2str(scale(j)*q(j)));
0055             else
0056                 % get value from text box
0057                 q(j) = str2num(get(gco, 'String')) / scale(j);
0058                 set(handles{1}, 'Value', q(j));
0059             end
0060             rr.q = q;
0061             set(r, 'UserData', rr);
0062             plot(rr, q)
0063         end
0064 
0065         % compute and display the T6 pose
0066         t6 = fkine(rr, q);
0067         h3 = get(findobj('Tag', 'T6'), 'UserData');
0068         for i=1:3,
0069             set(h3(i,1), 'String', sprintf('%.3f', t6(i,4)));
0070             set(h3(i,2), 'String', sprintf('%.3f', t6(i,3)));
0071         end
0072     else
0073         % drivebot(r, q)
0074         % drivebot(r, 'deg')
0075         r = a;
0076         scale = ones(r.n,1);
0077 
0078         n = r.n;
0079         width = 300;
0080         height = 40;
0081         minVal = -pi;
0082         maxVal = pi;    
0083 
0084         qlim = r.qlim;
0085         if isempty(qlim),
0086             qlim = [minVal*ones(r.n,1) maxVal*ones(r.n,1)];
0087         end
0088 
0089         if nargin < 2,
0090             q = zeros(1,n);
0091         else
0092             if isstr(b),
0093                 if strncmp(b, 'deg', 3),
0094                     disp('** in degree mode')
0095                     L = r.link;
0096                     for i=1:r.n,
0097                         if L{i}.sigma == 0,
0098                             scale(i) = 180/pi;
0099                         end
0100                     end
0101                 end
0102             else
0103                 q = b;
0104             end
0105         end
0106         t6 = fkine(r, q);
0107         fig = figure('Units', 'pixels', ...
0108             'Position', [0 -height width height*(n+2)], ...
0109             'Color', bgcol);
0110         set(fig,'MenuBar','none')
0111         delete( get(fig, 'Children') )
0112 
0113         % first we check to see if there are any graphical robots of
0114         % this name, if so we use them, otherwise create a robot plot.
0115 
0116         rh = findobj('Tag', r.name);
0117 
0118         % attempt to get current joint config of graphical robot
0119         if ~isempty(rh),
0120             rr = get(rh(1), 'UserData');
0121             if ~isempty(rr.q),
0122                 q = rr.q;
0123             end
0124         end
0125 
0126 
0127         % now make the sliders
0128         for i=1:n,
0129             uicontrol(fig, 'Style', 'text', ...
0130                 'Units', 'pixels', ...
0131                 'BackgroundColor', bgcol, ...
0132                 'Position', [0 height*(n-i) width*0.1 height*0.4], ...
0133                 'String', sprintf('q%d', i));
0134 
0135             h(i) = uicontrol(fig, 'Style', 'slider', ...
0136                 'Units', 'pixels', ...
0137                 'Position', [width*0.1 height*(n-i) width*0.7 height*0.4], ...
0138                 'Min', scale(i)*qlim(i,1), ...
0139                 'Max', scale(i)*qlim(i,2), ...
0140                 'Value', scale(i)*q(i), ...
0141                 'Tag', sprintf('Slider%d', i), ...
0142                 'Callback', ['drivebot(''' r.name ''',' num2str(i) ')']);
0143 
0144             h2(i) = uicontrol(fig, 'Style', 'edit', ...
0145                 'Units', 'pixels', ...
0146                 'Position', [width*0.8 height*(n-i-0.1) width*0.2 height*0.7], ...
0147                 'String', num2str(scale(i)*q(i)), ...
0148                 'Tag', sprintf('Edit%d', i), ...
0149                 'Callback', ['drivebot(''' r.name ''',' num2str(i) ')']);
0150 
0151             % hang handles off the slider and edit objects
0152             handles = {h(i) h2(i) scale};
0153             set(h(i), 'Userdata', handles);
0154             set(h2(i), 'Userdata', handles);
0155         end
0156 
0157         uicontrol(fig, 'Style', 'text', ...
0158             'Units', 'pixels', ...
0159             'FontSize', 20, ...
0160             'HorizontalAlignment', 'left', ...
0161             'Position', [0 height*(n+1) 0.8*width height], ...
0162             'BackgroundColor', 'white', ...
0163             'String', r.name);
0164 
0165         % X
0166         uicontrol(fig, 'Style', 'text', ...
0167             'Units', 'pixels', ...
0168             'BackgroundColor', bgcol, ...
0169             'Position', [0 height*(n+0.5) 0.06*width height/2], ...
0170             'BackgroundColor', 'yellow', ...
0171             'FontSize', 10, ...
0172             'HorizontalAlignment', 'left', ...
0173             'String', 'x:');
0174 
0175         h3(1,1) = uicontrol(fig, 'Style', 'edit', ...
0176             'Units', 'pixels', ...
0177             'Position', [0.06*width height*(n+0.5) width*0.2 height*0.6], ...
0178             'String', sprintf('%.3f', t6(1,4)), ...
0179             'Tag', 'T6');
0180 
0181         % Y
0182         uicontrol(fig, 'Style', 'text', ...
0183             'Units', 'pixels', ...
0184             'BackgroundColor', bgcol, ...
0185             'Position', [0.26*width height*(n+0.5) 0.06*width height/2], ...
0186             'BackgroundColor', 'yellow', ...
0187             'FontSize', 10, ...
0188             'HorizontalAlignment', 'left', ...
0189             'String', 'y:');
0190 
0191         h3(2,1) = uicontrol(fig, 'Style', 'edit', ...
0192             'Units', 'pixels', ...
0193             'Position', [0.32*width height*(n+0.5) width*0.2 height*0.6], ...
0194             'String', sprintf('%.3f', t6(2,4)));
0195 
0196         % Z
0197         uicontrol(fig, 'Style', 'text', ...
0198             'Units', 'pixels', ...
0199             'BackgroundColor', bgcol, ...
0200             'Position', [0.52*width height*(n+0.5) 0.06*width height/2], ...
0201             'BackgroundColor', 'yellow', ...
0202             'FontSize', 10, ...
0203             'HorizontalAlignment', 'left', ...
0204             'String', 'z:');
0205 
0206         h3(3,1) = uicontrol(fig, 'Style', 'edit', ...
0207             'Units', 'pixels', ...
0208             'Position', [0.58*width height*(n+0.5) width*0.2 height*0.6], ...
0209             'String', sprintf('%.3f', t6(3,4)));
0210 
0211         % AX
0212         uicontrol(fig, 'Style', 'text', ...
0213             'Units', 'pixels', ...
0214             'BackgroundColor', bgcol, ...
0215             'Position', [0 height*(n) 0.06*width height/2], ...
0216             'BackgroundColor', 'yellow', ...
0217             'FontSize', 10, ...
0218             'HorizontalAlignment', 'left', ...
0219             'String', 'ax:');
0220 
0221         h3(1,2) = uicontrol(fig, 'Style', 'edit', ...
0222             'Units', 'pixels', ...
0223             'Position', [0.06*width height*(n) width*0.2 height*0.6], ...
0224             'String', sprintf('%.3f', t6(1,3)));
0225 
0226         % AY
0227         uicontrol(fig, 'Style', 'text', ...
0228             'Units', 'pixels', ...
0229             'BackgroundColor', bgcol, ...
0230             'Position', [0.26*width height*(n) 0.06*width height/2], ...
0231             'BackgroundColor', 'yellow', ...
0232             'FontSize', 10, ...
0233             'HorizontalAlignment', 'left', ...
0234             'String', 'ay:');
0235 
0236         h3(2,2) = uicontrol(fig, 'Style', 'edit', ...
0237             'Units', 'pixels', ...
0238             'Position', [0.32*width height*(n) width*0.2 height*0.6], ...
0239             'String', sprintf('%.3f', t6(2,3)));
0240 
0241         % AZ
0242         uicontrol(fig, 'Style', 'text', ...
0243             'Units', 'pixels', ...
0244             'BackgroundColor', bgcol, ...
0245             'Position', [0.52*width height*(n) 0.06*width height/2], ...
0246             'BackgroundColor', 'yellow', ...
0247             'FontSize', 10, ...
0248             'HorizontalAlignment', 'left', ...
0249             'String', 'az:');
0250 
0251         h3(3,2) = uicontrol(fig, 'Style', 'edit', ...
0252             'Units', 'pixels', ...
0253             'Position', [0.58*width height*(n) width*0.2 height*0.6], ...
0254             'String', sprintf('%.3f', t6(3,3)));
0255 
0256 
0257         set(h3(1,1), 'Userdata', h3);
0258         uicontrol(fig, 'Style', 'pushbutton', ...
0259             'Units', 'pixels', ...
0260             'FontSize', 16, ...
0261             'Position', [0.8*width height*n 0.2*width 2*height], ...
0262             'CallBack', 'delete(gcf)', ...
0263             'BackgroundColor', 'red', ...
0264             'String', 'Quit');
0265 
0266 
0267         if isempty(rh),
0268             figure
0269             plot(r, q);
0270         end
0271     end

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