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');
end
%%
% This function computes the Inverse Kinematics for the Puma 762 robot
% given X,Y,Z coordinates for a point in the workspace. Note: The IK are
% computed for the origin of Coordinate systems 4,5 & 6.
function [theta1,theta2,theta3,theta4,theta5,theta6] = PumaIK(Px,Py,Pz)
theta4 = 0;
theta5 = 0;
theta6 = 0;
sign1 = 1;
sign3 = 1;
nogo = 0;
noplot = 0;
% Because the sqrt term in theta1 & theta3 can be + or - we run through
% all possible combinations (i = 4) and take the first combination that
% satisfies the joint angle constraints.
while nogo == 0;
for i = 1:1:4
if i == 1
sign1 = 1;
sign3 = 1;
elseif i == 2
sign1 = 1;
sign3 = -1;
elseif i == 3
sign1 = -1;
sign3 = 1;
else
sign1 = -1;
sign3 = -1;
end
a2 = 650;
a3 = 0;
d3 = 190;
d4 = 600;
rho = sqrt(Px^2+Py^2);
phi = atan2(Py,Px);
K = (Px^2+Py^2+Pz^2-a2^2-a3^2-d3^2-d4^2)/(2*a2);
c4 = cos(theta4);
s4 = sin(theta4);
c5 = cos(theta5);
s5 = sin(theta5);
c6 = cos(theta6);
s6 = sin(theta6);
theta1 = (atan2(Py,Px)-atan2(d3,sign1*sqrt(Px^2+Py^2-d3^2)));
c1 = cos(theta1);
s1 = sin(theta1);
theta3 = (atan2(a3,d4)-atan2(K,sign3*sqrt(a3^2+d4^2-K^2)));
c3 = cos(theta3);
s3 = sin(theta3);
t23 = atan2((-a3-a2*c3)*Pz-(c1*Px+s1*Py)*(d4-a2*s3),(a2*s3-d4)*Pz+(a3+a2*c3)*(c1*Px+s1*Py));
theta2 = (t23 - theta3);
c2 = cos(theta2);
s2 = sin(theta2);
s23 = ((-a3-a2*c3)*Pz+(c1*Px+s1*Py)*(a2*s3-d4))/(Pz^2+(c1*Px+s1*Py)^2);
c23 = ((a2*s3-d4)*Pz+(a3+a2*c3)*(c1*Px+s1*Py))/(Pz^2+(c1*Px+s1*Py)^2);
r13 = -c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
r23 = -s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
r33 = s23*c4*s5 - c23*c5;
theta4 = atan2(-r13*s1+r23*c1,-r13*c1*c23-r23*s1*c23+r33*s23);
r11 = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)+s1*(s4*c5*c6+c4*s6);
r21 = s1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-c1*(s4*c5*c6+c4*s6);
r31 = -s23*(c4*c5*c6-s4*s6)-c23*s5*c6;
s5 = -(r13*(c1*c23*c4+s1*s4)+r23*(s1*c23*c4-c1*s4)-r33*(s23*c4));
c5 = r13*(-c1*s23)+r23*(-s1*s23)+r33*(-c23);
theta5 = atan2(s5,c5);
s6 = -r11*(c1*c23*s4-s1*c4)-r21*(s1*c23*s4+c1*c4)+r31*(s23*s4);
c6 = r11*((c1*c23*c4+s1*s4)*c5-c1*s23*s5)+r21*((s1*c23*c4-c1*s4)*c5-s1*s23*s5)-r31*(s23*c4*c5+c23*s5);
theta6 = atan2(s6,c6);
theta1 = theta1*180/pi;
theta2 = theta2*180/pi;
theta3 = theta3*180/pi;
theta4 = theta4*180/pi;
theta5 = theta5*180/pi;
theta6 = theta6*180/pi;
if theta2>=160 && theta2<=180
theta2 = -theta2;
end
if theta1<=160 && theta1>=-160 && (theta2<=20 && theta2>=-200) && theta3<=45 && theta3>=-225 && theta4<=266 && theta4>=-266 && theta5<=100 && theta5>=-100 && theta6<=266 && theta6>=-266
nogo = 1;
theta3 = theta3+180;
break
end
if i == 4 && nogo == 0
h = errordlg('Point unreachable due to joint angle constraints.','JOINT ERROR');
waitfor(h);
nogo = 1;
noplot = 1;
break
end
end
end
end
%
%%
function pumaANI(theta1,theta2,theta3,theta4,theta5,theta6,n,trail)
% This function will animate the Puma 762 robot given joint angles.
% n is number of steps for the animation
% trail is 'y' or 'n' (n = anything else) for leaving a trail.
%
%disp('in animate');
a2 = 650; %D-H paramaters
a3 = 0;
d3 = 190;
d4 = 600;
% Err2 = 0;
%
ThetaOld = getappdata(0,'ThetaOld');
%
theta1old = ThetaOld(1);
theta2old = ThetaOld(2);
theta3old = ThetaOld(3);
theta4old = ThetaOld(4);
theta5old = ThetaOld(5);
theta6old = ThetaOld(6);
%
t1 = linspace(theta1old,theta1,n);
t2 = linspace(theta2old,theta2,n);
t3 = linspace(theta3old,theta3,n);% -180;
t4 = linspace(theta4old,theta4,n);
t5 = linspace(theta5old,theta5,n);
t6 = linspace(theta6old,theta6,n);
n = length(t1);
for i = 2:1:n
% Forward Kinematics
%
T_01 = tmat(0, 0, 0, t1(i));
T_12 = tmat(-90, 0, 0, t2(i));
T_23 = tmat(0, a2, d3, t3(i));
T_34 = tmat(-90, a3, d4, t4(i));
T_45 = tmat(90, 0, 0, t5(i));
T_56 = tmat(-90, 0, 0, t6(i));
%
% % T_67 = [ 1 0 0 0
% % 0 1 0 0
% % 0 0 1 188
% % 0 0 0 1];
%T_01 = T_01; % it is, but don't need to say so.
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;
% T_07 = T_06*T_67;
%
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');
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
|
Скачать 337.85 Kb.