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.
329 lines
10 KiB
329 lines
10 KiB
%SerialLink.teach Graphical teach pendant
|
|
%
|
|
% R.teach() 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 instanes of the graphical robots are driven.
|
|
%
|
|
% R.teach(Q) specifies the initial joint angle, otherwise it is taken from
|
|
% one of the existing graphical robots.
|
|
%
|
|
% See also SerialLink.plot.
|
|
|
|
|
|
% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
|
|
%
|
|
% 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 teach(r, varargin)
|
|
bgcol = [135 206 250]/255;
|
|
|
|
opt.degrees = false;
|
|
opt.q0 = [];
|
|
% TODO: options for rpy, or eul angle display
|
|
|
|
opt = tb_optparse(opt, varargin);
|
|
|
|
% drivebot(r, q)
|
|
% drivebot(r, 'deg')
|
|
scale = ones(r.n,1);
|
|
|
|
n = r.n;
|
|
width = 300;
|
|
height = 40;
|
|
minVal = -pi;
|
|
maxVal = pi;
|
|
|
|
qlim = r.qlim;
|
|
if isempty(qlim)
|
|
qlim = [minVal*ones(r.n,1) maxVal*ones(r.n,1)];
|
|
end
|
|
|
|
if isempty(opt.q0)
|
|
q = zeros(1,n);
|
|
else
|
|
q = opt.q0;
|
|
end
|
|
|
|
% set up scale factor
|
|
scale = [];
|
|
for L=r.links
|
|
if opt.degrees && L.revolute
|
|
scale = [scale 180/pi];
|
|
else
|
|
scale = [scale 1];
|
|
end
|
|
end
|
|
|
|
T6 = r.fkine(q);
|
|
fig = figure('Units', 'pixels', ...
|
|
'Position', [0 -height width height*(n+2)+20], ...
|
|
'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.
|
|
|
|
rh = findobj('Tag', r.name);
|
|
|
|
% attempt to get current joint config of graphical robot
|
|
if ~isempty(rh)
|
|
rr = get(rh(1), 'UserData');
|
|
if ~isempty(rr.q)
|
|
q = rr.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)+20 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
|
|
h(i) = uicontrol(fig, 'Style', 'slider', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [width*0.1 height*(n-i)+20 width*0.7 height*0.4], ...
|
|
'Min', scale(i)*qlim(i,1), ...
|
|
'Max', scale(i)*qlim(i,2), ...
|
|
'Value', scale(i)*q(i), ...
|
|
'Tag', sprintf('Slider%d', i), ...
|
|
'Callback', @(src,event)teach_callback(r.name, i));
|
|
|
|
% if findjobj exists use it, since it lets us get continous callbacks while
|
|
% a slider moves
|
|
if exist('findjobj') && ~ispc
|
|
drawnow
|
|
jh = findjobj(h(i),'nomenu');
|
|
jh.AdjustmentValueChangedCallback = {@sliderCallbackFunc, r.name, i};
|
|
end
|
|
% text box showing slider value, also editable
|
|
h2(i) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [width*0.8 height*(n-i-0.1)+20 width*0.2 height*0.7], ...
|
|
'String', num2str(scale(i)*q(i)), ...
|
|
'Tag', sprintf('Edit%d', i), ...
|
|
'Callback', @(src,event)teach_callback(r.name, i));
|
|
|
|
% hang handles off the slider and edit objects
|
|
handles = {h(i) h2(i) scale};
|
|
set(h(i), 'Userdata', handles);
|
|
set(h2(i), 'Userdata', handles);
|
|
end
|
|
|
|
% robot name text box
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'FontSize', 20, ...
|
|
'HorizontalAlignment', 'left', ...
|
|
'Position', [0 height*(n+1.2)+20 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)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'x:');
|
|
|
|
h3(1,1) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.06*width height*(n+0.5)+20 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)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'y:');
|
|
|
|
h3(2,1) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.32*width height*(n+0.5)+20 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)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'z:');
|
|
|
|
h3(3,1) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.58*width height*(n+0.5)+20 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(3,4)));
|
|
|
|
% AX
|
|
uicontrol(fig, 'Style', 'text', ...
|
|
'Units', 'pixels', ...
|
|
'BackgroundColor', bgcol, ...
|
|
'Position', [0 height*(n)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'ax:');
|
|
|
|
h3(1,2) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.06*width height*(n)+20 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)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'ay:');
|
|
|
|
h3(2,2) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.32*width height*(n)+20 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)+20 0.06*width height/2], ...
|
|
'BackgroundColor', 'yellow', ...
|
|
'FontSize', 10, ...
|
|
'HorizontalAlignment', 'right', ...
|
|
'String', 'az:');
|
|
|
|
h3(3,2) = uicontrol(fig, 'Style', 'edit', ...
|
|
'Units', 'pixels', ...
|
|
'Position', [0.58*width height*(n)+20 width*0.2 height*0.6], ...
|
|
'String', sprintf('%.3f', T6(3,3)));
|
|
|
|
|
|
set(h3(1,1), 'Userdata', h3);
|
|
uicontrol(fig, 'Style', 'pushbutton', ...
|
|
'Units', 'pixels', ...
|
|
'FontSize', 16, ...
|
|
'Position', [0.8*width height*n+20 0.2*width 2*height], ...
|
|
'CallBack', 'delete(gcf)', ...
|
|
'BackgroundColor', 'red', ...
|
|
'String', 'Quit');
|
|
|
|
|
|
if isempty(rh)
|
|
figure
|
|
r.plot(q);
|
|
end
|
|
end
|
|
|
|
function teach_callback(a, b)
|
|
% called on changes to a slider or to the edit box showing joint coordinate
|
|
% teach_callback(robot name, joint index)
|
|
|
|
name = a; % name of the robot
|
|
j = b; % joint index
|
|
|
|
% find all graphical objects tagged with the robot name, this is the
|
|
% instancs of that robot across all figures
|
|
rh = findobj('Tag', name);
|
|
|
|
% get the handles {slider, textbox, scale factor}
|
|
handles = get(gcbo, 'Userdata');
|
|
scale = handles{3};
|
|
|
|
for r=rh' % for every graphical robot instance
|
|
robot = get(r, 'UserData'); % get the robot object
|
|
q = robot.q; % get its current joint angles
|
|
if isempty(q)
|
|
q = zeros(1,robot.n);
|
|
end
|
|
|
|
if gcbo == handles{1}
|
|
% get value from slider
|
|
q(j) = get(gcbo, 'Value') / scale(j);
|
|
set(handles{2}, 'String', num2str(scale(j)*q(j)));
|
|
elseif gcbo == handles{2}
|
|
% get value from text box
|
|
q(j) = str2num(get(gcbo, 'String')) / scale(j);
|
|
set(handles{1}, 'Value', q(j));
|
|
else
|
|
warning('shouldnt happen');
|
|
end
|
|
robot.q = q;
|
|
set(r, 'UserData', robot);
|
|
robot.plot(q)
|
|
end
|
|
|
|
% compute and display the T6 pose
|
|
T6 = robot.fkine(q);
|
|
h3 = get(findobj('Tag', 'T6'), 'UserData');
|
|
for i=1:3
|
|
set(h3(i,1), 'String', sprintf('%.3f', T6(i,4)));
|
|
set(h3(i,2), 'String', sprintf('%.3f', T6(i,3)));
|
|
end
|
|
|
|
robot.T = T6;
|
|
robot.notify('Moved');
|
|
drawnow
|
|
end
|
|
|
|
function sliderCallbackFunc(src, ev, name, joint)
|
|
if get(src,'ValueIsAdjusting') == 1
|
|
try
|
|
teach_callback(name, joint);
|
|
drawnow
|
|
catch
|
|
return
|
|
end
|
|
end
|
|
end
|