s5 = getappdata(0,'Link5_data');
s6 = getappdata(0,'Link6_data');
s7 = getappdata(0,'Link7_data');
%A1 = getappdata(0,'Area_data');
Link1 = s1.V1;
Link2 = (T_01*s2.V2')';
Link3 = (T_02*s3.V3')';
Link4 = (T_03*s4.V4')';
Link5 = (T_04*s5.V5')';
Link6 = (T_05*s6.V6')';
Link7 = (T_06*s7.V7')';
% Tool = T_07;
% if sqrt(Tool(1,4)^2+Tool(2,4)^2)<514
% Err2 = 1;
% break
% end
%
handles = getappdata(0,'patch_h'); %
L1 = handles(1);
L2 = handles(2);
L3 = handles(3);
L4 = handles(4);
L5 = handles(5);
L6 = handles(6);
L7 = handles(7);
Tr = handles(9);
%
set(L1,'vertices',Link1(:,1:3),'facec', [0.717,0.116,0.123]);
set(L1, 'EdgeColor','none');
set(L2,'vertices',Link2(:,1:3),'facec', [0.216,1,.583]);
set(L2, 'EdgeColor','none');
set(L3,'vertices',Link3(:,1:3),'facec', [0.306,0.733,1]);
set(L3, 'EdgeColor','none');
set(L4,'vertices',Link4(:,1:3),'facec', [1,0.542,0.493]);
set(L4, 'EdgeColor','none');
set(L5,'vertices',Link5(:,1:3),'facec', [0.216,1,.583]);
set(L5, 'EdgeColor','none');
set(L6,'vertices',Link6(:,1:3),'facec', [1,1,0.255]);
set(L6, 'EdgeColor','none');
set(L7,'vertices',Link7(:,1:3),'facec', [0.306,0.733,1]);
set(L7, 'EdgeColor','none');
% store trail in appdata
if trail == 'y'
x_trail = getappdata(0,'xtrail');
y_trail = getappdata(0,'ytrail');
z_trail = getappdata(0,'ztrail');
%
xdata = [x_trail T_04(1,4)];
ydata = [y_trail T_04(2,4)];
zdata = [z_trail T_04(3,4)];
%
setappdata(0,'xtrail',xdata); % used for trail tracking.
setappdata(0,'ytrail',ydata); % used for trail tracking.
setappdata(0,'ztrail',zdata); % used for trail tracking.
%
set(Tr,'xdata',xdata,'ydata',ydata,'zdata',zdata);
end
drawnow
end
setappdata(0,'ThetaOld',[theta1,theta2,theta3,theta4,theta5,theta6]);
end
%%
%
%
%%
function InitHome
% Use forward kinematics to place the robot in a specified
% configuration.
% Figure setup data, create a new figure for the GUI
set(0,'Units','pixels')
dim = get(0,'ScreenSize');
fig_1 = figure('doublebuffer','on','Position',[0,35,dim(3)-200,dim(4)-110],...
'MenuBar','none','Name',' 3D Puma Robot Graphical Demo',...
'NumberTitle','off','CloseRequestFcn',@del_app);
hold on;
%light('Position',[-1 0 0]);
light % add a default light
daspect([1 1 1]) % Setting the aspect ratio
view(135,25)
xlabel('X'),ylabel('Y'),zlabel('Z');
title('WWU Robotics Lab PUMA 762');
axis([-1500 1500 -1500 1500 -1120 1500]);
plot3([-1500,1500],[-1500,-1500],[-1120,-1120],'k')
plot3([-1500,-1500],[-1500,1500],[-1120,-1120],'k')
plot3([-1500,-1500],[-1500,-1500],[-1120,1500],'k')
plot3([-1500,-1500],[1500,1500],[-1120,1500],'k')
plot3([-1500,1500],[-1500,-1500],[1500,1500],'k')
plot3([-1500,-1500],[-1500,1500],[1500,1500],'k')
s1 = getappdata(0,'Link1_data');
s2 = getappdata(0,'Link2_data');
s3 = getappdata(0,'Link3_data');
s4 = getappdata(0,'Link4_data');
s5 = getappdata(0,'Link5_data');
s6 = getappdata(0,'Link6_data');
s7 = getappdata(0,'Link7_data');
A1 = getappdata(0,'Area_data');
%
a2 = 650;
a3 = 0;
d3 = 190;
d4 = 600;
Px = 5000;
Py = 5000;
Pz = 5000;
%The 'home' position, for init.
t1 = 90;
t2 = -90;
t3 = -90;
t4 = 0;
t5 = 0;
t6 = 0;
% Forward Kinematics
T_01 = tmat(0, 0, 0, t1);
T_12 = tmat(-90, 0, 0, t2);
T_23 = tmat(0, a2, d3, t3);
T_34 = tmat(-90, a3, d4, t4);
T_45 = tmat(90, 0, 0, t5);
T_56 = tmat(-90, 0, 0, t6);
% Each link fram to base frame transformation
T_02 = T_01*T_12;
T_03 = T_02*T_23;
T_04 = T_03*T_34;
T_05 = T_04*T_45;
T_06 = T_05*T_56;
% Actual vertex data of robot links
Link1 = s1.V1;
Link2 = (T_01*s2.V2')';
Link3 = (T_02*s3.V3')';
Link4 = (T_03*s4.V4')';
Link5 = (T_04*s5.V5')';
Link6 = (T_05*s6.V6')';
Link7 = (T_06*s7.V7')';
% points are no fun to watch, make it look 3d.
L1 = patch('faces', s1.F1, 'vertices' ,Link1(:,1:3));
L2 = patch('faces', s2.F2, 'vertices' ,Link2(:,1:3));
L3 = patch('faces', s3.F3, 'vertices' ,Link3(:,1:3));
L4 = patch('faces', s4.F4, 'vertices' ,Link4(:,1:3));
L5 = patch('faces', s5.F5, 'vertices' ,Link5(:,1:3));
L6 = patch('faces', s6.F6, 'vertices' ,Link6(:,1:3));
L7 = patch('faces', s7.F7, 'vertices' ,Link7(:,1:3));
A1 = patch('faces', A1.Fa, 'vertices' ,A1.Va(:,1:3));
Tr = plot3(0,0,0,'b.'); % holder for trail paths
%
setappdata(0,'patch_h',[L1,L2,L3,L4,L5,L6,L7,A1,Tr])
%
setappdata(0,'xtrail',0); % used for trail tracking.
setappdata(0,'ytrail',0); % used for trail tracking.
setappdata(0,'ztrail',0); % used for trail tracking.
%
set(L1, 'facec', [0.717,0.116,0.123]);
set(L1, 'EdgeColor','none');
set(L2, 'facec', [0.216,1,.583]);
set(L2, 'EdgeColor','none');
set(L3, 'facec', [0.306,0.733,1]);
set(L3, 'EdgeColor','none');
set(L4, 'facec', [1,0.542,0.493]);
set(L4, 'EdgeColor','none');
set(L5, 'facec', [0.216,1,.583]);
set(L5, 'EdgeColor','none');
set(L6, 'facec', [1,1,0.255]);
set(L6, 'EdgeColor','none');
set(L7, 'facec', [0.306,0.733,1]);
set(L7, 'EdgeColor','none');
set(A1, 'facec', [.8,.8,.8],'FaceAlpha',.25);
set(A1, 'EdgeColor','none');
%
setappdata(0,'ThetaOld',[90,-90,-90,0,0,0]);
%
end
%%
function T = tmat(alpha, a, d, theta)
% tmat(alpha, a, d, theta) (T-Matrix used in Robotics)
% The homogeneous transformation called the "T-MATRIX"
% as used in the Kinematic Equations for robotic type
% systems (or equivalent).
%
% This is equation 3.6 in Craig's "Introduction to Robotics."
% alpha, a, d, theta are the Denavit-Hartenberg parameters.
%
% (NOTE: ALL ANGLES MUST BE IN DEGREES.)
%
alpha = alpha*pi/180; %Note: alpha is in radians.
theta = theta*pi/180; %Note: theta is in radians.
c = cos(theta);
s = sin(theta);
ca = cos(alpha);
sa = sin(alpha);
T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
end
%%
function del_app(varargin)
%This is the main figure window close function, to remove any
% app data that may be left due to using it for geometry.
%CloseRequestFcn
% here is the data to remove:
% Link1_data: [1x1 struct]
% Link2_data: [1x1 struct]
% Link3_data: [1x1 struct]
% Link4_data: [1x1 struct]
% Link5_data: [1x1 struct]
% Link6_data: [1x1 struct]
% Link7_data: [1x1 struct]
% Area_data: [1x1 struct]
% patch_h: [1x9 double]
% ThetaOld: [90 -182 -90 -106 80 106]
% xtrail: 0
% ytrail: 0
% ztrail: 0
% Now remove them.
rmappdata(0,'Link1_data');
rmappdata(0,'Link2_data');
rmappdata(0,'Link3_data');
rmappdata(0,'Link4_data');
rmappdata(0,'Link5_data');
rmappdata(0,'Link6_data');
rmappdata(0,'Link7_data');
rmappdata(0,'ThetaOld');
rmappdata(0,'Area_data');
rmappdata(0,'patch_h');
rmappdata(0,'xtrail');
rmappdata(0,'ytrail');
rmappdata(0,'ztrail');
delete(fig_1);
end
%%
function [hout,ax_out] = uibutton(varargin)
%uibutton: Create pushbutton with more flexible labeling than uicontrol.
% Usage:
% uibutton accepts all the same arguments as uicontrol except for the
% following property changes:
%
% Property Values
% ----------- ------------------------------------------------------
% Style 'pushbutton', 'togglebutton' or 'text', default =
% 'pushbutton'.
% String Same as for text() including cell array of strings and
% TeX or LaTeX interpretation.
% Interpreter 'tex', 'latex' or 'none', default = default for text()
%
% Syntax:
% handle = uibutton('PropertyName',PropertyValue,...)
% handle = uibutton(parent,'PropertyName',PropertyValue,...)
% [text_obj,axes_handle] = uibutton('Style','text',...
% 'PropertyName',PropertyValue,...)
%
% uibutton creates a temporary axes and text object containing the text to
% be displayed, captures the axes as an image, deletes the axes and then
% displays the image on the uicontrol. The handle to the uicontrol is
% returned. If you pass in a handle to an existing uicontol as the first
% argument then uibutton will use that uicontrol and not create a new one.
%
% If the Style is set to 'text' then the axes object is not deleted and the
% text object handle is returned (as well as the handle to the axes in a
% second output argument).
%
% See also UICONTROL.
% Version: 1.6, 20 April 2006
% Author: Douglas M. Schwarz
% Email: dmschwarz=ieee*org, dmschwarz=urgrad*rochester*edu
% Real_email = regexprep(Email,{'=','*'},{'@','.'})
% Detect if first argument is a uicontrol handle.
keep_handle = false;
if nargin > 0
h = varargin{1};
if isscalar(h) && ishandle(h) && strcmp(get(h,'Type'),'uicontrol')
keep_handle = true;
varargin(1) = [];
end
end
% Parse arguments looking for 'Interpreter' property. If found, note its
% value and then remove it from where it was found.
interp_value = get(0,'DefaultTextInterpreter');
arg = 1;
remove = [];
while arg <= length(varargin)
v = varargin{arg};
if isstruct(v)
fn = fieldnames(v);
for i = 1:length(fn)
if strncmpi(fn{i},'interpreter',length(fn{i}))
interp_value = v.(fn{i});
v = rmfield(v,fn{i});
end
end
varargin{arg} = v;
arg = arg + 1;
elseif ischar(v)
if strncmpi(v,'interpreter',length(v))
interp_value = varargin{arg+1};
remove = [remove,arg,arg+1];
end
arg = arg + 2;
elseif arg == 1 && isscalar(v) && ishandle(v) && ...
any(strcmp(get(h,'Type'),{'figure','uipanel'}))
arg = arg + 1;
else
error('Invalid property or uicontrol parent.')
end
end
varargin(remove) = [];
% Create uicontrol, get its properties then hide it.
if keep_handle
set(h,varargin{:})
else
h = uicontrol(varargin{:});
end
s = get(h);
if
any(strcmp(s.Style,{'pushbutton','togglebutton','text'}))
delete(h)
error('''Style'' must be pushbutton, togglebutton or text.')
end
set(h,'Visible','off')
% Create axes.
parent = get(h,'Parent');
ax = axes('Parent',parent,...
'Units',s.Units,...
'Position',s.Position,...
'XTick',[],'YTick',[],...
'XColor',s.BackgroundColor,...
'YColor',s.BackgroundColor,...
'Box','on',...
'Color',s.BackgroundColor);
% Adjust size of axes for best appearance.
set(ax,'Units','pixels')
pos = round(get(ax,'Position'));
if strcmp(s.Style,'text')
set(ax,'Position',pos + [0 1 -1 -1])
else
set(ax,'Position',pos + [4 4 -8 -8])
end
switch s.HorizontalAlignment
case 'left'
x = 0.0;
case 'center'
x = 0.5;
case 'right'
x = 1;
end
% Create text object.
text_obj = text('Parent',ax,...
'Position',[x,0.5],...
'String',s.String,...
'Interpreter',interp_value,...
'HorizontalAlignment',s.HorizontalAlignment,...
'VerticalAlignment','middle',...
'FontName',s.FontName,...
'FontSize',s.FontSize,...
'FontAngle',s.FontAngle,...
'FontWeight',s.FontWeight,...
'Color',s.ForegroundColor);
% If we are creating something that looks like a text uicontrol then we're
% all done and we return the text object and axes handles rather than a
% uicontrol handle.
if strcmp(s.Style,'text')
delete(h)
if nargout
hout = text_obj;
ax_out = ax;
end
return
end
% Capture image of axes and then delete the axes.
frame = getframe(ax);
delete(ax)
% Build RGB image, set background pixels to NaN and put it in 'CData' for
% the uicontrol.
if isempty(frame.colormap)
rgb = frame.cdata;
else
rgb = reshape(frame.colormap(frame.cdata,:),[pos([4,3]),3]);
end
size_rgb = size(rgb);
rgb = double(rgb)/255;
back = repmat(permute(s.BackgroundColor,[1 3 2]),size_rgb(1:2));
isback = all(rgb == back,3);
rgb(repmat(isback,[1 1 3])) = NaN;
set(h,'CData',rgb,'String','','Visible',s.Visible)
% Assign output argument if necessary.
if nargout
hout = h;
end
%%
end
end
% Finally.
demo = uicontrol(fig_1,'String','Demo','callback',@demo_button_press,...
'Position',[20 5 60 20]);
rnd_demo = uicontrol(fig_1,'String','Random Move','callback',@rnd_demo_button_press,...
'Position',[100 5 80 20]);
clr_trail = uicontrol(fig_1,'String','Clr Trail','callback',@clr_trail_button_press,...
'Position',[200 5 60 20]);
%
home = uicontrol(fig_1,'String','Home','callback',@home_button_press,...
'Position',[280 5 70 20]);
%
% Kinematics Panel
%
K_p = uipanel(fig_1,...
'units','pixels',...
'Position',[20 45 265 200],...
'Title','Kinematics','FontSize',11);
%
% Angle Range Default Name
% Theta 1: 320 (-160 to 160) 90 Waist Joint
% Theta 2: 220 (-110 to 110) -90 Shoulder Joint
% Theta 3: 270 (-135 to 135) -90 Elbow Joint
% Theta 4: 532 (-266 to 266) 0 Wrist Roll
% Theta 5: 200 (-100 to 100) 0 Wrist Bend
% Theta 6: 532 (-266 to 266) 0 Wrist Swivel
t1_home = 90; % offset to define the "home" position as UP.
t2_home = -90;
t3_home = -90;
LD = 105; % Left, used to set the GUI.
HT = 18; % Height
BT = 156; % Bottom
%% GUI buttons for Theta 1. pos is: [left bottom width height]
t1_slider = uicontrol(K_p,'style','slider',...
'Max',160,'Min',-160,'Value',0,...
'SliderStep',[0.05 0.2],...
'callback',@t1_slider_button_press,...
'Position',[LD BT 120 HT]);
t1_min = uicontrol(K_p,'style','text',...
'String','-160',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t1_max = uicontrol(K_p,'style','text',...
'String','+160',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t1_text = uibutton(K_p,'style','text',... % Nice program Doug. Need this
'String','\theta_1',... % due to no TeX in uicontrols.
'Position',[LD-100 BT 20 HT]); % L, B, W, H
% t1_text = uicontrol(K_p,'style','text',... % when matlab fixes uicontrol
% 'String','t1',... % for TeX, then I can use this.
% 'Position',[LD-100 BT 20 HT]); % L, B, W, H
t1_edit = uicontrol(K_p,'style','edit',...
'String',0,...
'callback',@t1_edit_button_press,...
'Position',[LD-75 BT 30 HT]); % L, B, W, H
%
%% GUI buttons for Theta 2.
BT = 126; % Bottom
t2_slider = uicontrol(K_p,'style','slider',...
'Max',115,'Min',-115,'Value',0,... % Mech. stop limits !
'SliderStep',[0.05 0.2],...
'callback',@t2_slider_button_press,...
'Position',[LD BT 120 HT]);
t2_min = uicontrol(K_p,'style','text',...
'String','-110',...
'Position',[LD-30 BT+1 25 HT-4]); % L, from bottom, W, H
t2_max = uicontrol(K_p,'style','text',...
'String','+110',...
'Position',[LD+125 BT+1 25 HT-4]); % L, B, W, H
t2_text = uibutton(K_p,'style','text',...
'String','\theta_2',...
|
Скачать 337.85 Kb.