基于视觉的PUMA560机械臂的抓取路径规划问题

function varargout = puma560_control(varargin) %初始化代码 gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @puma560_control_OpeningFcn, ... 'gui_OutputFcn', @puma560_control_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end %系统查询 function puma560_control_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); %初始化程序 puma_gui=findobj('Tag','puma560_gui'); %定位图形对象并返回Tag属性被设置为puma560_gui值的图形对象的句柄 if ~isempty(puma_gui), %存在PUMA560演示窗口 else %不存在PUMA560演示窗口 uiwait(msgbox('您还没有打开PUMA560演示窗口。程序将自动为您打开！','提示')); puma560_gui(); end function varargout = puma560_control_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function pushbtn_yunN_Callback(hObject, eventdata, handles) %运动学逆问题 eval('puma560'); close(findobj('tag','puma560_gui')); prompt={'输入时间向量：','输入起始点坐标:','输入终止点坐标：'}; name='参数：'; numlines=1; defaultanswer={'0:0.056:2','transl(0.6,-0.5,0)','transl(0.4,0.5,0.2)'}; answer=inputdlg(prompt,name,numlines,defaultanswer); if ~isempty(answer), t=str2num(answer{1}); T1=str2num(answer{2}); T2=str2num(answer{3}); end T=ctraj(T1,T2,length(t)); q=ikine(p560,T); figure('Name','PUMA560机器人仿真演示窗口---运动学逆问题'); subplot(3,2,1) plot(t,q(:,1)) xlabel('Time (s)'); ylabel('Joint 1 (rad)') subplot(3,2,2) plot(t,q(:,2)) xlabel('Time (s)'); ylabel('Joint 2 (rad)') subplot(3,2,3) plot(t,q(:,3)) xlabel('Time (s)'); ylabel('Joint 3 (rad)') subplot(3,2,4) plot(t,q(:,4)) xlabel('Time (s)'); ylabel('Joint 4 (rad)') subplot(3,2,5) plot(t,q(:,5)) xlabel('Time (s)'); ylabel('Joint 5 (rad)') subplot(3,2,6) plot(t,q(:,6)) xlabel('Time (s)'); ylabel('Joint 6 (rad)') eval('puma560'); puma_gui=guihandles(puma560_gui()); axes(puma_gui.axes1); plot(p560,q); function pushbtn_trace_Callback(hObject, eventdata, handles) %轨迹规划：关节空间(jtraj)，直角坐标(ctraj)和两种方案 str={'关节空间','直角坐标'}; [s,v]=listdlg('PromptString','选择方案:','SelectionMode','single','ListString',str); switch s, case 0, case 1, prompt={'输入时间向量:','输入起始点坐标:','输入终止点坐标：'}; name='输入所需参数'; numlines=1; defaultanswer={'0:0.056:2','[0 0 0 0 0 0]','[0 pi/2 -pi/2 0 0 0]'}; answer=inputdlg(prompt,name,numlines,defaultanswer); t=str2num(answer{1}); qz=str2num(answer{2}); qr=str2num(answer{3}); [q,qd,qdd]=jtraj(qz,qr,t); nq=find(all(q==0)==0); for i=1:length(nq), figure('Name','PUMA560机器人仿真演示窗口---轨迹规划(关节空间）'); subplot(3,1,1); plot(t,q(:,nq(i))); title('Theta'); xlabel('Time(s)'); ylabel(['Joint ' num2str(nq(i)) '(rad)']); subplot(3,1,2); plot(t,qd(:,nq(i))); title('Velocity'); xlabel('Time(s)'); ylabel(['Joint ' num2str(nq(i)) 'vel(rad/s)']); subplot(3,1,3); plot(t,q(:,nq(i))); title('Acceleration'); xlabel('Time(s)'); ylabel(['Joint ' num2str(nq(i)) 'accel(rad/s^2)']); end puma_gui=guihandles(puma560_gui()); axes(puma_gui.axes1); eval('puma560'); plot(p560,q); case 2, prompt={'输入时间向量：','输入起始点坐标:','输入终止点坐标：'}; name='输入所需参数：'; numlines=1; defaultanswer={'0:0.056:2','transl(0.6,-0.5,0)','transl(0.4,0.5,0.2)'}; answer=inputdlg(prompt,name,numlines,defaultanswer); if ~isempty(answer), t=str2num(answer{1}); T1=str2num(answer{2}); T2=str2num(answer{3}); end T=ctraj(T1,T2,length(t)); h=findobj('tag','puma560_gui'); if ~isempty(h), close(findobj('Tag','puma560_gui')); end figure('Name','PUMA560机器人仿真演示窗口---轨迹规划（直角坐标）'); subplot(3,1,1); plot(t, squeeze(T(1,4,:))); xlabel('Time (s)'); ylabel('X (m)'); subplot(3,1,2); plot(t, squeeze(T(2,4,:))); xlabel('Time (s)'); ylabel('Y (m)'); subplot(3,1,3); plot(t, squeeze(T(3,4,:))) xlabel('Time (s)'); ylabel('Z (m)'); figure('Name','PUMA560机器人仿真演示窗口---轨迹规划（直角坐标）'); plot3(squeeze(T(1,4,:)),squeeze(T(2,4,:)),squeeze(T(3,4,:))); eval('puma560'); q=ikine(p560,T); puma_gui=guihandles(puma560_gui()); axes(puma_gui.axes1); plot(p560,q); end function pushbtn_exit_Callback(hObject, eventdata, handles) %退出程序 close(findobj('Tag','puma560_control')); puma_gui=findobj('Tag','puma560_gui'); if ~isempty(puma_gui),close(puma_gui);end function pushbtn_import_Callback(hObject, eventdata, handles) %载入数据，调用puma560原程序 puma=findobj('Tag','puma560_gui'); if ~isempty(puma), %如果存在PUMA560演示窗口 puma=guihandles(puma); eval('puma560'); axes(puma.axes1); qz=zeros(1,6); plot(p560,qz); else %如果不存在PUMA560演示窗口 puma=guihandles(puma560_gui()); axes(puma.axes1); qz=zeros(1,6); eval('puma560'); plot(p560,qz); end function pushbtn_animation_Callback(hObject, eventdata, handles) %动画演示程序，puma560机械臂末端按平行四边形运动 T1=transl(0.6,-0.5,0.2); T2=transl(0.6,0.5,0.2); T3=transl(0.6,0.5,-0.2); T4=transl(0.6,-0.5,-0.2); T={T1,T2,T3,T4,T1}; eval('puma560'); for i=1:1, for j=1:4, start_point=T{j}; end_point=T{j+1}; T0=ctraj(start_point,end_point,36); qianhe=findobj('Tag','qianhe'); if isempty(qianhe), qianhe=figure('Name','PUMA560机器人仿真演示窗口（三维坐标）','Tag','qianhe'); figure(qianhe); plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:))); hold on; else hold on; figure(qianhe); plot3(squeeze(T0(1,4,:)),squeeze(T0(2,4,:)),squeeze(T0(3,4,:))); end q=ikine(p560,T0); puma=findobj('Tag','puma560_gui'); if ~isempty(puma), puma=guihandles(puma); axes(puma.axes1); cla; plot(p560,q); else puma=guihandles(puma560_gui); axes(puma.axes1); cla; plot(p560,q); end end end function slider_q1_Callback(hObject, eventdata, handles) % 第一关节的状态设置 val=get(hObject,'Value'); set(handles.edit_q1,'String',num2str(val)); drive_robot(handles); function slider_q1_CreateFcn(hObject, eventdata, handles) % 第一关节的命令栏设置 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]); end function slider_q2_Callback(hObject, eventdata, handles) val=get(hObject,'Value'); set(handles.edit_q2,'String',num2str(val)); drive_robot(handles); function slider_q2_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]); end function slider_q3_Callback(hObject, eventdata, handles) val=get(hObject,'Value'); set(handles.edit_q3,'String',num2str(val)); drive_robot(handles); function slider_q3_CreateFcn(hObject, eventdata, handles) if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor',[.9 .9 .9]); end function slider_q4_Callback(hObject, eventdata, handles) val=get(hObject,'Value'); set(handles.edit_q4,'String',num2str(val)); drive_robot(handles); function slider_q4_CreateFcn(hObject, even