robotsim.asv

是一个在matlab下的机器人运动仿真的软件

function varargout = RobotSim(varargin)
% ROBOTSIM M-file for RobotSim.fig
%      ROBOTSIM, by itself, creates a new ROBOTSIM or raises the existing
%      singleton*.
%
%      H = ROBOTSIM returns the handle to a new ROBOTSIM or the handle to
%      the existing singleton*.
%
%      ROBOTSIM('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in ROBOTSIM.M with the given input arguments.
%
%      ROBOTSIM('Property','Value',...) creates a new ROBOTSIM or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before RobotSim_OpeningFunction gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to RobotSim_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 RobotSim

% Last Modified by GUIDE v2.5 16-Feb-2008 08:46:14

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

%% Opening
% --- Executes just before RobotSim is made visible.
function RobotSim_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 RobotSim (see VARARGIN)

[a,map,b]=imread('_Rodar_Off.JPG'); %IMREAD Read image from graphics file.
set(handles.tbRodar,'CData',a);
[a,map,b]=imread('_Zoom_Off.JPG');
set(handles.tbZoom,'CData',a);
[a,map,b]=imread('_Cursor_Off.JPG');
set(handles.tbCursor,'CData',a);
[a,map,b]=imread('_Reset_.JPG');
set(handles.tbReset,'CData',a);

set(hObject, 'Units', 'pixels');
handles.robotimagem = imread( 'robotimagem','jpeg'); % Read the image file
info = imfinfo('robotimagem','jpeg'); % Determine the size of the image file
%position = get(hObject, 'Position');
%set(hObject, 'Position', [position(1:2) info.Width + 100 info.Height+100])
axes(handles.axes3);%Create axes in arbitrary positions.
axis off;
image(handles.robotimagem)
set(handles.axes3,'Visible', 'off','Units', 'pixels');

axes(handles.axes2);
axis on;
axis([-100 100 -100 100 -100 100]); %Get a window to show the robot

Initial_veriable;    %Call initial veriable function
Initial_Skeleton;    %Call the function that initializes robot skeleton
%-------------------------------------------------------------------------
%This is very important for the whole project. It is the algorithm for
%calculating movement.如果是但建立一个文件，应该如何在axse中调用还不知道。比如
%Initial_Skeleton????????????????????????????????????
global face1base face2base face3base face4base face5base face6base...
       face1column1 face2column1 face3column1 face4column1 face5column1 face6column1...
       face1top face2top face3top face4top face5top face6top...
       face1small1 face2small1 face3small1 face4small1 face5small1 face6small1...
       face1column2 face2column2 face3column2 face4column2 face5column2 ...
       face1small2 face2small2 face3small2 face4small2 face5small2 face6small2...
       face1column3 face2column3 face3column3 face4column3 face5column3 face6column3...
       face1wrist face2wrist face3wrist face4wrist face5wrist face6wrist...
       face1cluth face2cluth face3cluth face4cluth face5cluth face6cluth...
       face1tool face2tool face3tool face4tool face5tool face6tool
   
global theta1 theta2 theta3 theta4 d1 d2 d3 l1 T06


%axes(handles.axes2);
cla;
grid on; 
hold on;
axis([-300 300 -400 400 -30 350]);

T01=[cos(theta1),-sin(theta1),0,0;
    sin(theta1),cos(theta1),0,0;
    0,0,1,0;
    0,0,0,1];

T12=[1,0,0,0;
    0,1,0,0;
    0,0,1,d1;
    0,0,0,1];

T23=[1,0,0,l1;
    0,0,-1,-d2;
    0,1,0,0;
    0,0,0,1];

T34=[cos(theta2),-sin(theta2),0,0;
    sin(theta2),cos(theta2),0,0;
    0,0,1,d3;
    0,0,0,1];

T45=[cos(theta3),-sin(theta3),0,0;
    0,0,1,0;
    -sin(theta3),-cos(theta3),0,0;
    0,0,0,1];

T56=[cos(theta4),-sin(theta4),0,0;
    0,0,-1,0;
    sin(theta4),cos(theta4),0,0;
    0,0,0,1];

T06=T01*T12*T23*T34*T45*T56;

%% build up the base face
fill3(face1base(:,1),face1base(:,2),face1base(:,3),'g'); 
fill3(face2base(:,1),face2base(:,2),face2base(:,3),'g');
fill3(face3base(:,1),face3base(:,2),face3base(:,3),'g');
fill3(face4base(:,1),face4base(:,2),face4base(:,3),'g');
fill3(face5base(:,1),face5base(:,2),face5base(:,3),'g');
fill3(face6base(:,1),face6base(:,2),face6base(:,3),'g');

%% build up the column 1
   obj1column1=T01*transp(face1column1);
   obj2column1=T01*transp(face2column1);
   obj3column1=T01*transp(face3column1);
   obj4column1=T01*transp(face4column1);
   obj5column1=T01*transp(face5column1);
   obj6column1=T01*transp(face6column1);
   
   fill3(obj1column1(1,:),obj1column1(2,:),obj1column1(3,:),'b');
   fill3(obj2column1(1,:),obj2column1(2,:),obj2column1(3,:),'b');
   fill3(obj3column1(1,:),obj3column1(2,:),obj3column1(3,:),'b');
   fill3(obj4column1(1,:),obj4column1(2,:),obj4column1(3,:),'b');
   fill3(obj5column1(1,:),obj5column1(2,:),obj5column1(3,:),'b');
   fill3(obj6column1(1,:),obj6column1(2,:),obj6column1(3,:),'b');
   
%% build up the top column on column 1
   obj1top=T01*transp(face1top);
   obj2top=T01*transp(face2top);
   obj3top=T01*transp(face3top);
   obj4top=T01*transp(face4top);
   obj5top=T01*transp(face5top);
   obj6top=T01*transp(face6top);
   
   fill3(obj1top(1,:),obj1top(2,:),obj1top(3,:),'b');
   fill3(obj2top(1,:),obj2top(2,:),obj2top(3,:),'b');
   fill3(obj3top(1,:),obj3top(2,:),obj3top(3,:),'b');
   fill3(obj4top(1,:),obj4top(2,:),obj4top(3,:),'b');
   fill3(obj5top(1,:),obj5top(2,:),obj5top(3,:),'b');
   fill3(obj6top(1,:),obj6top(2,:),obj6top(3,:),'b');
   
%% build up the small part1 on column2
obj1small1=T01*T12*transp(face1small1);
obj2small1=T01*T12*transp(face2small1);
obj3small1=T01*T12*transp(face3small1);
obj4small1=T01*T12*transp(face4small1);
obj5small1=T01*T12*transp(face5small1);
obj6small1=T01*T12*transp(face6small1);

fill3(obj1small1(1,:),obj1small1(2,:),obj1small1(3,:),'y');
fill3(obj2small1(1,:),obj2small1(2,:),obj2small1(3,:),'y');
fill3(obj3small1(1,:),obj3small1(2,:),obj3small1(3,:),'y');
fill3(obj4small1(1,:),obj4small1(2,:),obj4small1(3,:),'y');
fill3(obj5small1(1,:),obj5small1(2,:),obj5small1(3,:),'y');
fill3(obj6small1(1,:),obj6small1(2,:),obj6small1(3,:),'y');

%% build up column2
obj1column2=T01*T12*transp(face1column2);
obj2column2=T01*T12*transp(face2column2);
obj3column2=T01*T12*transp(face3column2);
obj4column2=T01*T12*transp(face4column2);
obj5column2=T01*T12*transp(face5column2);

fill3(obj1column2(1,:),obj1column2(2,:),obj1column2(3,:),'y');
fill3(obj2column2(1,:),obj2column2(2,:),obj2column2(3,:),'y');
fill3(obj3column2(1,:),obj3column2(2,:),obj3column2(3,:),'y');
fill3(obj4column2(1,:),obj4column2(2,:),obj4column2(3,:),'y');
fill3(obj5column2(1,:),obj5column2(2,:),obj5column2(3,:),'y');

%% build up the small part2 on column2
obj1small2=T01*T12*transp(face1small2);
obj2small2=T01*T12*transp(face2small2);
obj3small2=T01*T12*transp(face3small2);
obj4small2=T01*T12*transp(face4small2);
obj5small2=T01*T12*transp(face5small2);
obj6small2=T01*T12*transp(face6small2);
   
fill3(obj1small2(1,:),obj1small2(2,:),obj1small2(3,:),'y');
fill3(obj2small2(1,:),obj2small2(2,:),obj2small2(3,:),'y');
fill3(obj3small2(1,:),obj3small2(2,:),obj3small2(3,:),'y');
fill3(obj4small2(1,:),obj4small2(2,:),obj4small2(3,:),'y');
fill3(obj5small2(1,:),obj5small2(2,:),obj5small2(3,:),'y');
fill3(obj6small2(1,:),obj6small2(2,:),obj6small2(3,:),'y');

%% build up the column 3
obj1column3=T01*T12*T23*transp(face1column3);
obj2column3=T01*T12*T23*transp(face2column3);
obj3column3=T01*T12*T23*transp(face3column3);
obj4column3=T01*T12*T23*transp(face4column3);
obj5column3=T01*T12*T23*transp(face5column3);
obj6column3=T01*T12*T23*transp(face6column3);

fill3(obj1column3(1,:),obj1column3(2,:),obj1column3(3,:),'r');
fill3(obj2column3(1,:),obj2column3(2,:),obj2column3(3,:),'r');
fill3(obj3column3(1,:),obj3column3(2,:),obj3column3(3,:),'r');
fill3(obj4column3(1,:),obj4column3(2,:),obj4column3(3,:),'r');
fill3(obj5column3(1,:),obj5column3(2,:),obj5column3(3,:),'r');
fill3(obj6column3(1,:),obj6column3(2,:),obj6column3(3,:),'r');

%% build up the wrist
 obj1wrist=T01*T12*T23*T34*transp(face1wrist);
 obj2wrist=T01*T12*T23*T34*transp(face2wrist);
 obj3wrist=T01*T12*T23*T34*transp(face3wrist);
 obj4wrist=T01*T12*T23*T34*transp(face4wrist);
 obj5wrist=T01*T12*T23*T34*transp(face5wrist);
 obj6wrist=T01*T12*T23*T34*transp(face6wrist);
     
fill3(obj1wrist(1,:),obj1wrist(2,:),obj1wrist(3,:),'g');
fill3(obj2wrist(1,:),obj2wrist(2,:),obj2wrist(3,:),'g');
fill3(obj3wrist(1,:),obj3wrist(2,:),obj3wrist(3,:),'g');
fill3(obj4wrist(1,:),obj4wrist(2,:),obj4wrist(3,:),'g');
fill3(obj5wrist(1,:),obj5wrist(2,:),obj5wrist(3,:),'g');
fill3(obj6wrist(1,:),obj6wrist(2,:),obj6wrist(3,:),'g');

%% build up the cluth
 obj1cluth=T01*T12*T23*T34*T45*transp(face1cluth);
 obj2cluth=T01*T12*T23*T34*T45*transp(face2cluth);
 obj3cluth=T01*T12*T23*T34*T45*transp(face3cluth);
 obj4cluth=T01*T12*T23*T34*T45*transp(face4cluth);
 obj5cluth=T01*T12*T23*T34*T45*transp(face5cluth);
 obj6cluth=T01*T12*T23*T34*T45*transp(face6cluth);  
     
fill3(obj1cluth(1,:),obj1cluth(2,:),obj1cluth(3,:),'b');
fill3(obj2cluth(1,:),obj2cluth(2,:),obj2cluth(3,:),'b');
fill3(obj3cluth(1,:),obj3cluth(2,:),obj3cluth(3,:),'b');
fill3(obj4cluth(1,:),obj4cluth(2,:),obj4cluth(3,:),'b');
fill3(obj5cluth(1,:),obj5cluth(2,:),obj5cluth(3,:),'b');
fill3(obj6cluth(1,:),obj6cluth(2,:),obj6cluth(3,:),'b');

%% build up the tool
 obj1tool=T01*T12*T23*T34*T45*T56*transp(face1tool);
 obj2tool=T01*T12*T23*T34*T45*T56*transp(face2tool);
 obj3tool=T01*T12*T23*T34*T45*T56*transp(face3tool);
 obj4tool=T01*T12*T23*T34*T45*T56*transp(face4tool);
 obj5tool=T01*T12*T23*T34*T45*T56*transp(face5tool);
 obj6tool=T01*T12*T23*T34*T45*T56*transp(face6tool);  
     

fill3(obj1tool(1,:),obj1tool(2,:),obj1tool(3,:),'c');
fill3(obj2tool(1,:),obj2tool(2,:),obj2tool(3,:),'c');
fill3(obj3tool(1,:),obj3tool(2,:),obj3tool(3,:),'c');
fill3(obj4tool(1,:),obj4tool(2,:),obj4tool(3,:),'c');
fill3(obj5tool(1,:),obj5tool(2,:),obj5tool(3,:),'c');
fill3(obj6tool(1,:),obj6tool(2,:),obj6tool(3,:),'c');
%-------------------------------------------------------------------------
%Initialize cimetic
set(handles.edittheta1,'String','0');
set(handles.edittheta2,'String','90');
set(handles.edittheta3,'String','0');
set(handles.edittheta4,'String','0');
set(handles.editd1,'String','90');
set(handles.editd2,'String','125');
set(handles.textd1,'String',get(handles.editd1,'String'));
set(handles.textd2,'String',get(handles.editd2,'String'));
set(handles.texttheta1,'String',get(handles.edittheta1,'String'));
set(handles.texttheta2,'String',get(handles.edittheta2,'String'));
set(handles.texttheta3,'String',get(handles.edittheta3,'String'));
set(handles.texttheta4,'String',get(handles.edittheta4,'String'));
%--------------------------------------------------------------------------
%还未定义Indirect选项的初始化

% Choose default command line output for RobotSim
handles.output = hObject;
guidata(hObject, handles);% Update handles structure





%% Output
% --- Outputs from this function are returned to the command line.
function varargout = RobotSim_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;

%% Callback
function edit1_Callback(hObject, eventdata, handles)
% hObject    handle to edit1 (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 edit1 as text
%        str2double(get(hObject,'String')) returns contents of edit1 as a double


% --- Executes during object creation, after setting all properties.
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit1 (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 edit2_Callback(hObject, eventdata, handles)
% hObject    handle to edit2 (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 edit2 as text
%        str2double(get(hObject,'String')) returns contents of edit2 as a double


% --- Executes during object creation, after setting all properties.