You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
432 lines
14 KiB
432 lines
14 KiB
%SerialLink.teach Graphical teach pendant
|
|
%
|
|
% R.teach(OPTIONS) drive a graphical robot by means of a graphical slider panel.
|
|
% If no graphical robot exists one is created in a new window. Otherwise
|
|
% all current instances of the graphical robot are driven.
|
|
%
|
|
% h = R.teach(OPTIONS) as above but returns a handle for the teach window. Can
|
|
% be used to programmatically delete the teach window.
|
|
%
|
|
% Options::
|
|
% 'eul' Display tool orientation in Euler angles
|
|
% 'rpy' Display tool orientation in roll/pitch/yaw angles
|
|
% 'approach' Display tool orientation as approach vector (z-axis)
|
|
% 'degrees' Display angles in degrees (default radians)
|
|
% 'q0',q Set initial joint coordinates
|
|
% 'callback',C Set a callback function
|
|
%
|
|
% GUI::
|
|
%
|
|
% - The record button adds the current joint coordinates as a row to the robot's
|
|
% qteach property.
|
|
% - The Quit button destroys the teach window.
|
|
%
|
|
% Notes::
|
|
% - The slider limits are derived from the joint limit properties. If not
|
|
% set then for
|
|
% - a revolute joint they are assumed to be [-pi, +pi]
|
|
% - a prismatic joint they are assumed unknown and an error occurs.
|
|
%
|
|
% See also SerialLink.plot.
|
|
|
|
% Copyright (C) 1993-2011, by Peter I. Corke
|
|
%
|
|
% This file is part of The Robotics Toolbox for Matlab (RTB).
|
|
%
|
|
% RTB is free software: you can redistribute it and/or modify
|
|
% it under the terms of the GNU Lesser General Public License as published by
|
|
% the Free Software Foundation, either version 3 of the License, or
|
|
% (at your option) any later version.
|
|
%
|
|
% RTB is distributed in the hope that it will be useful,
|
|
% but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
% GNU Lesser General Public License for more details.
|
|
%
|
|
% You should have received a copy of the GNU Leser General Public License
|
|
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
|
|
%
|
|
% http://www.petercorke.com
|
|
|
|
%% TODO:
|
|
%% make the sliders change the animation while moving
|
|
%% http://www.mathworks.com/matlabcentral/newsreader/view_thread/159374
|
|
%% 1. download FINDJOBJ from the file exchange: http://www.mathworks.com/matlabcentral/fileexchange/14317
|
|
%% 2. hSlider = uicontrol('style','slider', ...); %create the slider, get its Matlab handle
|
|
%% 3. jSlider = findjobj(hSlider,'nomenu'); %get handle of the underlying java object
|
|
%% 4. jSlider.AdjustmentValueChangedCallback = @myMatlabFunction; %set callback
|
|
%%
|
|
%% Note: you can also use the familiar format:
|
|
%% set(jSlider,'AdjustmentValueChangedCallback',@myMatlabFunction)
|
|
%%
|
|
%% Feel free to explore many other properties (and ~30 callbacks)
|
|
%% available in jSlider but not in its Matlab front-end interface hSlider.
|
|
%%
|
|
%% Warning: FINDJOBJ relies on undocumented and unsupported Matlab/Java
|
|
%% functionality.
|
|
|
|
function handle = teach(r, varargin)
|
|
bgcol = [135 206 250]/255;
|
|
|
|
opt.degrees = false;
|
|
opt.q0 = [];
|
|
opt.orientation = {'approach', 'eul', 'rpy'};
|
|
opt.callback = [];
|
|
% TODO: options for rpy, or eul angle display
|
|
|
|
opt = tb_optparse(opt, varargin);
|
|
|
|
% drivebot(r, q)
|
|
% drivebot(r, 'deg')
|
|
|
|
n = r.n;
|
|
width = 300;
|
|
height = 40;
|
|
|
|
qlim = r.qlim;
|
|
if any(isinf(qlim))
|
|
error('for prismatic axes must define joint coordinate limits, set qlim properties for prismatic Links');
|
|
end
|
|
|
|
if isempty(opt.q0)
|
|
q = zeros(1,n);
|
|
else
|
|
q = opt.q0;
|
|
end
|
|
|
|
% set up scale factor, from actual limits in radians/metres to display units
|
|
qscale = ones(r.n,1);
|
|
for i=1:r.n
|
|
L=r.links(i);
|
|
if opt.degrees && L.isrevolute
|
|
qscale(i) = 180/pi;
|
|
end
|
|
end
|
|
|
|
handles.qscale = qscale;
|
|
handles.orientation = opt.orientation;
|
|
handles.callback = opt.callback;
|
|
|
|
T6 = r.fkine(q);
|
|
|
|
fig = figure('Units', 'pixels', ...
|
|
'Position', [0 -height width height*(n+2)+40], ...
|
|
'BusyAction', 'cancel', ...
|
|
'HandleVisibility', 'off', ...
|
|
'Color', bgcol);
|
|
set(fig,'MenuBar','none')
|
|
delete( get(fig, 'Children') )
|
|
|
|
% first we check to see if there are any graphical robots of
|
|
% this name, if so we use them, otherwise create a robot plot.
|
|
|
|
rhandles = findobj('Tag', r.name);
|
|
|
|
% attempt to get current joint config of graphical robot
|
|
if ~isempty(rhandles)
|
|
h = get(rhandles(1), 'UserData');
|
|
if ~isempty(h.q)
|
|
q = h.q;
|
|
end
|
|
end
|
|
|
|
% now make the sliders
|
|
for i=1:n
|
|
% slider label
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0 height*(n-i)+40 width*0.1 height*0.4], ...
|
|
'String', sprintf('q%d', i));
|
|
|
|
% slider itself
|
|
q(i) = max( qlim(i,1), min( qlim(i,2), q(i) ) ); % clip to range
|
|
handles.slider(i) = uicontrol(fig, 'Style', 'slider', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [width*0.1 height*(n-i)+40 width*0.7 height*0.4], ...
|
|
'Min', qlim(i,1), ...
|
|
'Max', qlim(i,2), ...
|
|
'Value', q(i), ...
|
|
'Tag', sprintf('Slider%d', i));
|
|
|
|
% text box showing slider value, also editable
|
|
handles.edit(i) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [width*0.8 height*(n-i-0.1)+40 width*0.2 height*0.7], ...
|
|
'String', num2str(qscale(i)*q(i)), ...
|
|
'Tag', sprintf('Edit%d', i));
|
|
end
|
|
|
|
set(handles.slider(1))
|
|
|
|
|
|
% robot name text box
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'FontSize', 20, ...
|
|
'HorizontalAlignment', 'left', ...
|
|
'Position', [0 height*(n+1.2)+40 0.8*width 0.8*height], ...
|
|
'BackgroundColor', 'white', ...
|
|
'String', r.name);
|
|
|
|
% X
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0 height*(n+0.5)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'x:');
|
|
|
|
handles.t6.t(1) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.06*width height*(n+0.5)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(1,4)), ...
|
|
'Tag', 'T6');
|
|
|
|
% Y
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0.26*width height*(n+0.5)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'y:');
|
|
|
|
handles.t6.t(2) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.32*width height*(n+0.5)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(2,4)));
|
|
|
|
% Z
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0.52*width height*(n+0.5)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'z:');
|
|
|
|
handles.t6.t(3) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.58*width height*(n+0.5)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(3,4)));
|
|
|
|
% Orientation
|
|
switch opt.orientation
|
|
case 'approach'
|
|
labels = {'ax:', 'ay:', 'az:'};
|
|
case 'eul'
|
|
labels = {'\phi:', '\theta:', '\psi:'};
|
|
case'rpy'
|
|
labels = {'R:', 'P:', 'Y:'};
|
|
end
|
|
% AX
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0 height*(n)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', labels(1));
|
|
|
|
handles.t6.r(1) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.06*width height*(n)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(1,3)));
|
|
|
|
% AY
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0.26*width height*(n)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', labels(2));
|
|
|
|
handles.t6.r(2) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.32*width height*(n)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(2,3)));
|
|
|
|
% AZ
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0.52*width height*(n)+40 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', labels(2));
|
|
|
|
handles.t6.r(3) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.58*width height*(n)+40 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(3,3)));
|
|
|
|
% add buttons
|
|
uicontrol(fig, 'Style', 'pushbutton', ...
|
|
'Units', 'pixels', ...
|
|
'FontSize', 16, ...
|
|
'Position', [0.8*width height*n+40 0.2*width 2*height], ...
|
|
'CallBack', @(src,event) delete(fig), ...
|
|
'BackgroundColor', 'red', ...
|
|
'String', 'Quit');
|
|
% the record button
|
|
handles.record = [];
|
|
uicontrol(fig, 'Style', 'pushbutton', ...
|
|
'Units', 'pixels', ...
|
|
'FontSize', 15, ...
|
|
'Position', [0.05*width 8 0.9*width 0.7*height], ...
|
|
'CallBack', @(src,event) record_callback(r, handles), ...
|
|
'BackgroundColor', 'blue', ...
|
|
'ForegroundColor', 'white', ...
|
|
'String', 'record');
|
|
|
|
|
|
% now assign the callbacks
|
|
for i=1:n
|
|
% text edit box
|
|
set(handles.edit(i), ...
|
|
'Interruptible', 'off', ...
|
|
'Callback', @(src,event)teach_callback(src, r.name, i, handles));
|
|
|
|
% slider
|
|
set(handles.slider(i), ...
|
|
'Interruptible', 'off', ...
|
|
'BusyAction', 'queue', ...
|
|
'Callback', @(src,event)teach_callback(src, r.name, i, handles));
|
|
|
|
% if findjobj exists use it, since it lets us get continous callbacks while
|
|
% a slider moves
|
|
if (exist('findjobj', 'file')>0) && verLessThan('matlab', '8') && ~ispc
|
|
disp('using findjobj');
|
|
pause(0.1);
|
|
drawnow
|
|
jh = findjobj(handles.slider(i), 'nomenu');
|
|
%jh.AdjustmentValueChangedCallback = {@sliderCallbackFunc, r.name, i};
|
|
jh.AdjustmentValueChangedCallback = @(src,event)sliderCallbackFunc(src, handles.slider(i), r.name, i, handles);
|
|
end
|
|
end
|
|
|
|
if isempty(rhandles)
|
|
figure
|
|
r.plot(q);
|
|
end
|
|
|
|
if nargout > 0
|
|
handle = fig;
|
|
end
|
|
end
|
|
|
|
function teach_callback(src, name, j, handles)
|
|
% called on changes to a slider or to the edit box showing joint coordinate
|
|
%
|
|
% src the object that caused the event
|
|
% name name of the robot
|
|
% j the joint index concerned (1..N)
|
|
% slider true if the
|
|
|
|
qscale = handles.qscale;
|
|
|
|
switch get(src, 'Style')
|
|
case 'slider'
|
|
% slider changed, get value and reflect it to edit box
|
|
newval = get(src, 'Value');
|
|
set(handles.edit(j), 'String', num2str(qscale(j)*newval));
|
|
case 'edit'
|
|
% edit box changed, get value and reflect it to slider
|
|
newval = str2double(get(src, 'String')) / qscale(j);
|
|
set(handles.slider(j), 'Value', newval);
|
|
end
|
|
%fprintf('newval %d %f\n', j, newval);
|
|
|
|
|
|
% find all graphical objects tagged with the robot name, this is the
|
|
% instancs of that robot across all figures
|
|
rhandles = findobj('Tag', name);
|
|
|
|
for rhandle=rhandles'
|
|
% for every graphical robot instance
|
|
|
|
h = get(rhandle, 'UserData'); % get the robot object
|
|
robot = h.robot;
|
|
q = h.q; % get its current joint angles
|
|
if isempty(q)
|
|
q = zeros(1,robot.n);
|
|
end
|
|
q(j) = newval; % update the joint angle
|
|
|
|
% update the robot object
|
|
%robot.q = q;
|
|
%set(r, 'UserData', robot);
|
|
|
|
robot.plot(q); % plot it
|
|
drawnow
|
|
end
|
|
|
|
% compute the robot tool pose
|
|
T6 = robot.fkine(q);
|
|
|
|
% convert orientation to desired format
|
|
switch handles.orientation
|
|
case 'approach'
|
|
orient = T6(:,3); % approach vector
|
|
case 'eul'
|
|
orient = tr2eul(T6);
|
|
case'rpy'
|
|
orient = tr2rpy(T6);
|
|
end
|
|
|
|
% update the display in the teach window
|
|
for i=1:3
|
|
set(handles.t6.t(i), 'String', sprintf('%.3f', T6(i,4)));
|
|
set(handles.t6.r(i), 'String', sprintf('%.3f', orient(i)));
|
|
end
|
|
|
|
if ~isempty(handles.callback)
|
|
handles.callback(q);
|
|
end
|
|
|
|
robot.T = T6;
|
|
robot.notify('Moved');
|
|
%drawnow
|
|
end
|
|
|
|
function record_callback(robot, handles)
|
|
rhandles = findobj('Tag', robot.name);
|
|
|
|
if ~isempty(rhandles)
|
|
h = get(rhandles(1), 'UserData'); % get the plot graphics
|
|
h.q
|
|
robot.record(h.q);
|
|
end
|
|
end
|
|
|
|
function sliderCallbackFunc(src, h, name, joint, handles)
|
|
persistent busy
|
|
|
|
% this is really ugly but it works
|
|
if busy
|
|
return
|
|
end
|
|
|
|
if get(src,'ValueIsAdjusting') == 1
|
|
busy = true;
|
|
try
|
|
teach_callback(h, name, joint, handles);
|
|
catch me
|
|
fprintf('*******\n')
|
|
busy = false;
|
|
return
|
|
end
|
|
end
|
|
busy = false;
|
|
|
|
|
|
end
|