MATLAB机器人简单正逆运动学GUI演示

1、前言：机器人正逆运动学可以直接查看机器人工具箱的函数ikine和fkine,以及直接用teach（）函数演示正逆运动学。

如下：

L_1=20;
L_2=50;
L_3=40;
L(1)=Link([0 L_1 0 pi/2]);
L(2)=Link([0 0 L_2 0]);
L(3)=Link([0 0 L_3 0 ]);
Robot=SerialLink(L);

teach(Robot)

2、当在GUI中主要是数据输入求解的演示：（下面直接贴出代码），主要看

btn_Forward和btn_Inverse    %GUI的fig文件可以按照下图自己建立，也可直接下载我的资源：
                           <利用MATLAB和Robot toolbox 做的一个简单的三轴机器人GUI>btn_Inverse    %GUI的fig文件可以按照下图自己建立，也可直接下载我的资源：
                           <利用MATLAB和Robot toolbox 做的一个简单的三轴机器人GUI>

function varargout = SimpleRobotGui(varargin)
% SIMPLEROBOTGUI MATLAB code for SimpleRobotGui.fig
%      SIMPLEROBOTGUI, by itself, creates a new SIMPLEROBOTGUI or raises the existing
%      singleton*.
%
%      H = SIMPLEROBOTGUI returns the handle to a new SIMPLEROBOTGUI or the handle to
%      the existing singleton*.
%
%      SIMPLEROBOTGUI('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in SIMPLEROBOTGUI.M with the given input arguments.
%
%      SIMPLEROBOTGUI('Property','Value',...) creates a new SIMPLEROBOTGUI or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before SimpleRobotGui_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to SimpleRobotGui_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help SimpleRobotGui

% Last Modified by GUIDE v2.5 19-Jun-2018 16:36:17

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @SimpleRobotGui_OpeningFcn, ...
                   'gui_OutputFcn',  @SimpleRobotGui_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
% End initialization code - DO NOT EDIT


% --- Executes just before SimpleRobotGui is made visible.
function SimpleRobotGui_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to SimpleRobotGui (see VARARGIN)

% Choose default command line output for SimpleRobotGui
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes SimpleRobotGui wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = SimpleRobotGui_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;



function Theta_1_Callback(hObject, eventdata, handles)
% hObject    handle to Theta_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Theta_1 as text
%        str2double(get(hObject,'String')) returns contents of Theta_1 as a double


% --- Executes during object creation, after setting all properties.
function Theta_1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Theta_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function Theta_2_Callback(hObject, eventdata, handles)
% hObject    handle to Theta_2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Theta_2 as text
%        str2double(get(hObject,'String')) returns contents of Theta_2 as a double


% --- Executes during object creation, after setting all properties.
function Theta_2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Theta_2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function Theta_3_Callback(hObject, eventdata, handles)
% hObject    handle to Theta_3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Theta_3 as text
%        str2double(get(hObject,'String')) returns contents of Theta_3 as a double


% --- Executes during object creation, after setting all properties.
function Theta_3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Theta_3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function Pos_X_Callback(hObject, eventdata, handles)
% hObject    handle to Pos_X (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Pos_X as text
%        str2double(get(hObject,'String')) returns contents of Pos_X as a double


% --- Executes during object creation, after setting all properties.
function Pos_X_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Pos_X (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function Pos_Y_Callback(hObject, eventdata, handles)
% hObject    handle to Pos_Y (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Pos_Y as text
%        str2double(get(hObject,'String')) returns contents of Pos_Y as a double


% --- Executes during object creation, after setting all properties.
function Pos_Y_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Pos_Y (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function Pos_Z_Callback(hObject, eventdata, handles)
% hObject    handle to Pos_Z (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of Pos_Z as text
%        str2double(get(hObject,'String')) returns contents of Pos_Z as a double


% --- Executes during object creation, after setting all properties.
function Pos_Z_CreateFcn(hObject, eventdata, handles)
% hObject    handle to Pos_Z (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in btn_Forward.
function btn_Forward_Callback(hObject, eventdata, handles)
% hObject    handle to btn_Forward (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
Th_1=str2double(handles.Theta_1.String)*pi/180;
Th_2=str2double(handles.Theta_2.String)*pi/180;
Th_3=str2double(handles.Theta_3.String)*pi/180;

L_1=20;
L_2=50;
L_3=40;

L(1)=Link([0 L_1 0 pi/2]);
L(2)=Link([0 0 L_2 0]);
L(3)=Link([0 0 L_3 0 ]);

Robot=SerialLink(L);
Robot.name='RRR_Robot';
Robot.plot([Th_1 Th_2 Th_3]);

T=Robot.fkine([Th_1 Th_2 Th_3]);
handles.Pos_X.String=num2str(floor(T(1,4)));
handles.Pos_Y.String=num2str(floor(T(2,4)));
handles.Pos_Z.String=num2str(floor(T(3,4)));


% --- Executes on button press in btn_Inverse.
function btn_Inverse_Callback(hObject, eventdata, handles)
% hObject    handle to btn_Inverse (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
PX=str2double(handles.Pos_X.String);
PY=str2double(handles.Pos_Y.String);
PZ=str2double(handles.Pos_Z.String);

L_1=20;
L_2=50;
L_3=40;

L(1)=Link([0 L_1 0 pi/2]);
L(2)=Link([0 0 L_2 0]);
L(3)=Link([0 0 L_3 0 ]);

Robot=SerialLink(L);
Robot.name='RRR_Robot';

T=[1 0 0 PX;
   0 1 0 PY;
   0 0 1 PZ;
   0 0 0 1;]

J=Robot.ikine(T,'mask',[1 1 1 0 0 0])*180/pi;%J=Robot.ikine(T,[0 0 0],[1 1 1 0 0 0])*180/pi;原来的逆解出错，改下格式对少于6自由度的机器人求逆解
handles.Theta_1.String=num2str(floor(J(1)));
handles.Theta_2.String=num2str(floor(J(2)));
handles.Theta_3.String=num2str(floor(J(3)));
Robot.plot(J*pi/180);