teach

 TEACH MATLAB code for teach.fig
      TEACH, by itself, creates a new TEACH or raises the existing
      singleton*.

      H = TEACH returns the handle to a new TEACH or the handle to
      the existing singleton*.

      TEACH('CALLBACK',hObject,eventData,handles,...) calls the local
      function named CALLBACK in TEACH.M with the given input arguments.

      TEACH('Property','Value',...) creates a new TEACH or raises the
      existing singleton*.  Starting from the left, property value pairs are
      applied to the GUI before teach_OpeningFcn gets called.  An
      unrecognized property name or invalid value makes property application
      stop.  All inputs are passed to teach_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

0001 function varargout = teach(varargin)
0002 %
0003 % TEACH MATLAB code for teach.fig
0004 %      TEACH, by itself, creates a new TEACH or raises the existing
0005 %      singleton*.
0006 %
0007 %      H = TEACH returns the handle to a new TEACH or the handle to
0008 %      the existing singleton*.
0009 %
0010 %      TEACH('CALLBACK',hObject,eventData,handles,...) calls the local
0011 %      function named CALLBACK in TEACH.M with the given input arguments.
0012 %
0013 %      TEACH('Property','Value',...) creates a new TEACH or raises the
0014 %      existing singleton*.  Starting from the left, property value pairs are
0015 %      applied to the GUI before teach_OpeningFcn gets called.  An
0016 %      unrecognized property name or invalid value makes property application
0017 %      stop.  All inputs are passed to teach_OpeningFcn via varargin.
0018 %
0019 %      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
0020 %      instance to run (singleton)".
0021 %
0022 % See also: GUIDE, GUIDATA, GUIHANDLES
0023 
0024 % Copyright (C) 2012, by Arturo Gil Aparicio
0025 %
0026 % This file is part of ARTE (A Robotics Toolbox for Education).
0027 %
0028 % ARTE is free software: you can redistribute it and/or modify
0029 % it under the terms of the GNU Lesser General Public License as published by
0030 % the Free Software Foundation, either version 3 of the License, or
0031 % (at your option) any later version.
0032 %
0033 % ARTE is distributed in the hope that it will be useful,
0034 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0035 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0036 % GNU Lesser General Public License for more details.
0037 %
0038 % You should have received a copy of the GNU Leser General Public License
0039 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0040 
0041 % Edit the above text to modify the response to help teach
0042 
0043 % Last Modified by GUIDE v2.5 07-Nov-2013 17:17:00
0044 global configuration robot 
0045 global controls program
0046 
0047 
0048 if ~isfield(robot,'q') 
0049     warndlg('No robot variable found, please load a robot with: robot=load_robot','!! Warning !!')
0050     return;
0051 end
0052 
0053 % Begin initialization code - DO NOT EDIT
0054 gui_Singleton = 1;
0055 gui_State = struct('gui_Name',       mfilename, ...
0056                    'gui_Singleton',  gui_Singleton, ...
0057                    'gui_OpeningFcn', @teach_OpeningFcn, ...
0058                    'gui_OutputFcn',  @teach_OutputFcn, ...
0059                    'gui_LayoutFcn',  [] , ...
0060                    'gui_Callback',   []);
0061 if nargin && ischar(varargin{1})
0062     gui_State.gui_Callback = str2func(varargin{1});
0063 end
0064 
0065 if nargout
0066     [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});    
0067 else
0068     gui_mainfcn(gui_State, varargin{:});
0069 end
0070 % End initialization code - DO NOT EDIT
0071 
0072   
0073 
0074 % --- Executes just before teach is made visible.
0075 function teach_OpeningFcn(hObject, eventdata, handles, varargin)
0076 % This function has no output args, see OutputFcn.
0077 % hObject    handle to figure
0078 % eventdata  reserved - to be defined in a future version of MATLAB
0079 % handles    structure with handles and user data (see GUIDATA)
0080 % varargin   command line arguments to teach (see VARARGIN)
0081 
0082 % Choose default command line output for teach
0083 handles.output = hObject;
0084 
0085 % Update handles structure
0086 guidata(hObject, handles);
0087 
0088 % UIWAIT makes teach wait for user response (see UIRESUME)
0089 % uiwait(handles.figure1);
0090 
0091 %initialize application variables
0092 global robot configuration controls targets program
0093 
0094 %reset program
0095 program=[];
0096 program.program_counter=0;
0097 program.n_lines=0; %counter for the number of matlab lines
0098 
0099 program.rapid_lines=[];
0100 program.matlab_lines=[];
0101 
0102 %reset targets
0103 targets=[];
0104 
0105 controls.num_target_points=0;
0106 controls.global_gui_handle=hObject;
0107 
0108 
0109 robot.q=zeros(1,robot.DOF)';
0110 
0111 %reset position velocity acceleration and time
0112 robot.q_vector=[];
0113 robot.qd_vector=[];
0114 robot.qdd_vector=[];
0115 robot.time=[];
0116 
0117 
0118 %update T and Q in the program
0119 update_T_Q()
0120 update_sliders();
0121 
0122 figure(configuration.figure.robot);
0123 
0124 disp('Select the desired view for your robot')
0125 drawrobot3d(robot, robot.q);
0126 
0127 % --- Outputs from this function are returned to the command line.
0128 function varargout = teach_OutputFcn(hObject, eventdata, handles) 
0129 % varargout  cell array for returning output args (see VARARGOUT);
0130 % hObject    handle to figure
0131 % eventdata  reserved - to be defined in a future version of MATLAB
0132 % handles    structure with handles and user data (see GUIDATA)
0133 
0134 % Get default command line output from handles structure
0135 varargout{1} = handles.output;
0136 
0137 
0138 % --- Executes on selection change in popupmenu_instruction.
0139 function popupmenu_instruction_Callback(hObject, eventdata, handles)
0140 % hObject    handle to popupmenu_instruction (see GCBO)
0141 % eventdata  reserved - to be defined in a future version of MATLAB
0142 % handles    structure with handles and user data (see GUIDATA)
0143 
0144 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_instruction contents as cell array
0145 %        contents{get(hObject,'Value')} returns selected item from popupmenu_instruction
0146 
0147 
0148 % --- Executes during object creation, after setting all properties.
0149 function popupmenu_instruction_CreateFcn(hObject, eventdata, handles)
0150 % hObject    handle to popupmenu_instruction (see GCBO)
0151 % eventdata  reserved - to be defined in a future version of MATLAB
0152 % handles    empty - handles not created until after all CreateFcns called
0153 global controls
0154 
0155 controls.popupmenu_instruction=hObject;
0156 
0157 
0158 % Hint: popupmenu controls usually have a white background on Windows.
0159 %       See ISPC and COMPUTER.
0160 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0161     set(hObject,'BackgroundColor','white');
0162 end
0163 
0164 
0165 % --- Executes on selection change in listbox1.
0166 function listbox1_Callback(hObject, eventdata, handles)
0167 % hObject    handle to listbox1 (see GCBO)
0168 % eventdata  reserved - to be defined in a future version of MATLAB
0169 % handles    structure with handles and user data (see GUIDATA)
0170 
0171 % Hints: contents = cellstr(get(hObject,'String')) returns listbox1 contents as cell array
0172 %        contents{get(hObject,'Value')} returns selected item from listbox1
0173 
0174 
0175 % --- Executes during object creation, after setting all properties.
0176 function listbox1_CreateFcn(hObject, eventdata, handles)
0177 % hObject    handle to listbox1 (see GCBO)
0178 % eventdata  reserved - to be defined in a future version of MATLAB
0179 % handles    empty - handles not created until after all CreateFcns called
0180 
0181 % Hint: listbox controls usually have a white background on Windows.
0182 %       See ISPC and COMPUTER.
0183 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0184     set(hObject,'BackgroundColor','white');
0185 end
0186 
0187 
0188 % --- Executes on selection change in popupmenu_target_point.
0189 function popupmenu_target_point_Callback(hObject, eventdata, handles)
0190 % hObject    handle to popupmenu_target_point (see GCBO)
0191 % eventdata  reserved - to be defined in a future version of MATLAB
0192 % handles    structure with handles and user data (see GUIDATA)
0193 
0194 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_target_point contents as cell array
0195 %        contents{get(hObject,'Value')} returns selected item from popupmenu_target_point
0196 
0197 
0198 % --- Executes during object creation, after setting all properties.
0199 function popupmenu_target_point_CreateFcn(hObject, eventdata, handles)
0200 % hObject    handle to popupmenu_target_point (see GCBO)
0201 % eventdata  reserved - to be defined in a future version of MATLAB
0202 % handles    empty - handles not created until after all CreateFcns called
0203 
0204 global controls
0205 controls.popupmenu_target_point=hObject;
0206 
0207 
0208 % Hint: popupmenu controls usually have a white background on Windows.
0209 %       See ISPC and COMPUTER.
0210 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0211     set(hObject,'BackgroundColor','white');
0212 end
0213 
0214 
0215 % --- Executes on selection change in popupmenu_through_point.
0216 function popupmenu_through_point_Callback(hObject, eventdata, handles)
0217 % hObject    handle to popupmenu_through_point (see GCBO)
0218 % eventdata  reserved - to be defined in a future version of MATLAB
0219 % handles    structure with handles and user data (see GUIDATA)
0220 
0221 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_through_point contents as cell array
0222 %        contents{get(hObject,'Value')} returns selected item from popupmenu_through_point
0223 
0224 
0225 % --- Executes during object creation, after setting all properties.
0226 function popupmenu_through_point_CreateFcn(hObject, eventdata, handles)
0227 % hObject    handle to popupmenu_through_point (see GCBO)
0228 % eventdata  reserved - to be defined in a future version of MATLAB
0229 % handles    empty - handles not created until after all CreateFcns called
0230 global controls
0231 controls.popupmenu_through_point=hObject;
0232 
0233 % Hint: popupmenu controls usually have a white background on Windows.
0234 %       See ISPC and COMPUTER.
0235 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0236     set(hObject,'BackgroundColor','white');
0237 end
0238 
0239 
0240 % --- Executes on selection change in popupmenu_speed.
0241 function popupmenu_speed_Callback(hObject, eventdata, handles)
0242 % hObject    handle to popupmenu_speed (see GCBO)
0243 % eventdata  reserved - to be defined in a future version of MATLAB
0244 % handles    structure with handles and user data (see GUIDATA)
0245 
0246 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_speed contents as cell array
0247 %        contents{get(hObject,'Value')} returns selected item from popupmenu_speed
0248 
0249 
0250 % --- Executes during object creation, after setting all properties.
0251 function popupmenu_speed_CreateFcn(hObject, eventdata, handles)
0252 % hObject    handle to popupmenu_speed (see GCBO)
0253 % eventdata  reserved - to be defined in a future version of MATLAB
0254 % handles    empty - handles not created until after all CreateFcns called
0255 
0256 global controls
0257 controls.popupmenu_speed=hObject;
0258 
0259 % Hint: popupmenu controls usually have a white background on Windows.
0260 %       See ISPC and COMPUTER.
0261 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0262     set(hObject,'BackgroundColor','white');
0263 end
0264 
0265 
0266 % --- Executes on button press in pushbutton1.
0267 function pushbutton1_Callback(hObject, eventdata, handles)
0268 % hObject    handle to pushbutton1 (see GCBO)
0269 % eventdata  reserved - to be defined in a future version of MATLAB
0270 % handles    structure with handles and user data (see GUIDATA)
0271 
0272 
0273 % --- Executes on selection change in popupmenu_precision.
0274 function popupmenu_precision_Callback(hObject, eventdata, handles)
0275 % hObject    handle to popupmenu_precision (see GCBO)
0276 % eventdata  reserved - to be defined in a future version of MATLAB
0277 % handles    structure with handles and user data (see GUIDATA)
0278 
0279 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_precision contents as cell array
0280 %        contents{get(hObject,'Value')} returns selected item from popupmenu_precision
0281 
0282 
0283 % --- Executes during object creation, after setting all properties.
0284 function popupmenu_precision_CreateFcn(hObject, eventdata, handles)
0285 % hObject    handle to popupmenu_precision (see GCBO)
0286 % eventdata  reserved - to be defined in a future version of MATLAB
0287 % handles    empty - handles not created until after all CreateFcns called
0288 global controls
0289 controls.popupmenu_precision=hObject;
0290 % Hint: popupmenu controls usually have a white background on Windows.
0291 %       See ISPC and COMPUTER.
0292 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0293     set(hObject,'BackgroundColor','white');
0294 end
0295 
0296 
0297 % --- Executes on slider movement.
0298 function slider_q1_Callback(hObject, eventdata, handles)
0299 % hObject    handle to slider_q1 (see GCBO)
0300 % eventdata  reserved - to be defined in a future version of MATLAB
0301 % handles    structure with handles and user data (see GUIDATA)
0302 
0303 % Hints: get(hObject,'Value') returns position of slider
0304 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0305 
0306 global configuration robot controls
0307 slider_value = get(hObject,'Value');
0308 set(controls.edit_q1, 'String', num2str(slider_value));
0309 %convert to rads and store
0310 if robot.kind(1)=='R'
0311     robot.q(1) = slider_value*pi/180;%rad
0312 else
0313     robot.q(1) = slider_value;%m
0314 end
0315 
0316 update_T_Q();
0317 figure(configuration.figure.robot);
0318 drawrobot3d(robot, robot.q);
0319 draw_target_points();
0320 
0321 % --- Executes during object creation, after setting all properties.
0322 function slider_q1_CreateFcn(hObject, eventdata, handles)
0323 % hObject    handle to slider_q1 (see GCBO)
0324 % eventdata  reserved - to be defined in a future version of MATLAB
0325 % handles    empty - handles not created until after all CreateFcns called
0326 global robot controls
0327 controls.slider_q1=hObject;
0328 
0329 
0330 if robot.DOF>0
0331     %modify the limits of the slider object to fit max and min limits
0332     set(hObject, 'Min',robot.maxangle(1,1)*180/pi,'Max',robot.maxangle(1,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0333 else 
0334     disp('\nPlease set robot.DOF correspondingly')
0335 end
0336 
0337 % Hint: slider controls usually have a light gray background.
0338 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0339     set(hObject,'BackgroundColor',[.9 .9 .9]);
0340 end
0341 
0342 
0343 
0344 
0345 % --- Executes on slider movement.
0346 function slider_q2_Callback(hObject, eventdata, handles)
0347 % hObject    handle to slider_q2 (see GCBO)
0348 % eventdata  reserved - to be defined in a future version of MATLAB
0349 % handles    structure with handles and user data (see GUIDATA)
0350 
0351 % Hints: get(hObject,'Value') returns position of slider
0352 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0353 global configuration robot controls
0354 slider_value = get(hObject,'Value');
0355 set(controls.edit_q2, 'String', num2str(slider_value));
0356 
0357 %convert to rads and store
0358 if robot.kind(2)=='R'
0359     robot.q(2) = slider_value*pi/180;%rad
0360 else
0361     robot.q(2) = slider_value;%m
0362 end
0363 
0364 update_T_Q();
0365 figure(configuration.figure.robot);
0366 drawrobot3d(robot, robot.q);
0367 draw_target_points();
0368 
0369 
0370 % --- Executes during object creation, after setting all properties.
0371 function slider_q2_CreateFcn(hObject, eventdata, handles)
0372 % hObject    handle to slider_q2 (see GCBO)
0373 % eventdata  reserved - to be defined in a future version of MATLAB
0374 % handles    empty - handles not created until after all CreateFcns called
0375 
0376 global robot controls
0377 controls.slider_q2=hObject;
0378 
0379 
0380 if robot.DOF>1
0381     %modify the limits of the slider object to fit max and min limits
0382     set(hObject, 'Min',robot.maxangle(2,1)*180/pi,'Max',robot.maxangle(2,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0383 else 
0384     %make the control disappear
0385     set(hObject, 'Min',0,'Max',0, 'Value',0);
0386     disp('\nPlease set robot.DOF correspondingly')
0387 end
0388 
0389 % Hint: slider controls usually have a light gray background.
0390 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0391     set(hObject,'BackgroundColor',[.9 .9 .9]);
0392 end
0393 
0394 
0395 % --- Executes on slider movement.
0396 function slider_q3_Callback(hObject, eventdata, handles)
0397 % hObject    handle to slider_q3 (see GCBO)
0398 % eventdata  reserved - to be defined in a future version of MATLAB
0399 % handles    structure with handles and user data (see GUIDATA)
0400 
0401 % Hints: get(hObject,'Value') returns position of slider
0402 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0403 
0404 global configuration robot controls
0405 slider_value = get(hObject,'Value');
0406 set(controls.edit_q3, 'String', num2str(slider_value));
0407 %convert to rads and store
0408 if robot.kind(3)=='R'
0409     robot.q(3) = slider_value*pi/180;%rad
0410 else
0411     robot.q(3) = slider_value;%m
0412 end
0413 figure(configuration.figure.robot);
0414 update_T_Q();
0415 drawrobot3d(robot, robot.q);
0416 draw_target_points();
0417 
0418 % --- Executes during object creation, after setting all properties.
0419 function slider_q3_CreateFcn(hObject, eventdata, handles)
0420 % hObject    handle to slider_q3 (see GCBO)
0421 % eventdata  reserved - to be defined in a future version of MATLAB
0422 % handles    empty - handles not created until after all CreateFcns called
0423 
0424 
0425 global robot controls
0426 controls.slider_q3=hObject;
0427 
0428 if robot.DOF>2
0429     %modify the limits of the slider object to fit max and min limits
0430     set(hObject, 'Min',robot.maxangle(3,1)*180/pi,'Max',robot.maxangle(3,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0431 else 
0432     %make the control disappear
0433      set(hObject, 'Min',0,'Max',0, 'Value',0);
0434     disp('\nPlease set robot.DOF correspondingly')
0435 end
0436 
0437 % Hint: slider controls usually have a light gray background.
0438 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0439     set(hObject,'BackgroundColor',[.9 .9 .9]);
0440 end
0441 
0442 
0443 % --- Executes on slider movement.
0444 function slider_q4_Callback(hObject, eventdata, handles)
0445 % hObject    handle to slider_q4 (see GCBO)
0446 % eventdata  reserved - to be defined in a future version of MATLAB
0447 % handles    structure with handles and user data (see GUIDATA)
0448 
0449 % Hints: get(hObject,'Value') returns position of slider
0450 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0451 
0452 global configuration robot controls
0453 slider_value = get(hObject,'Value');
0454 set(controls.edit_q4, 'String', num2str(slider_value));
0455 %convert to rads and store
0456 if robot.kind(4)=='R'
0457     robot.q(4) = slider_value*pi/180;%rad
0458 else
0459     robot.q(4) = slider_value;%m
0460 end
0461 
0462 update_T_Q();
0463 figure(configuration.figure.robot);
0464 drawrobot3d(robot, robot.q);
0465 draw_target_points();
0466 
0467 % --- Executes during object creation, after setting all properties.
0468 function slider_q4_CreateFcn(hObject, eventdata, handles)
0469 % hObject    handle to slider_q4 (see GCBO)
0470 % eventdata  reserved - to be defined in a future version of MATLAB
0471 % handles    empty - handles not created until after all CreateFcns called
0472 
0473 
0474 global robot controls
0475 controls.slider_q4=hObject;
0476 
0477 if robot.DOF>3
0478     %modify the limits of the slider object to fit max and min limits
0479     set(hObject, 'Min',robot.maxangle(4,1)*180/pi,'Max',robot.maxangle(4,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0480 else 
0481     %make the control disappear
0482      set(hObject, 'Min',0,'Max',0, 'Value',0);
0483     disp('\nPlease set robot.DOF correspondingly')
0484 end
0485 
0486 
0487 % Hint: slider controls usually have a light gray background.
0488 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0489     set(hObject,'BackgroundColor',[.9 .9 .9]);
0490 end
0491 
0492 
0493 % --- Executes on slider movement.
0494 function slider_q5_Callback(hObject, eventdata, handles)
0495 % hObject    handle to slider_q5 (see GCBO)
0496 % eventdata  reserved - to be defined in a future version of MATLAB
0497 % handles    structure with handles and user data (see GUIDATA)
0498 
0499 % Hints: get(hObject,'Value') returns position of slider
0500 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0501 
0502 global configuration robot controls
0503 slider_value = get(hObject,'Value');
0504 set(controls.edit_q5, 'String', num2str(slider_value));
0505 %convert to rads and store
0506 if robot.kind(5)=='R'
0507     robot.q(5) = slider_value*pi/180;%rad
0508 else
0509     robot.q(5) = slider_value;%m
0510 end
0511 update_T_Q();
0512 figure(configuration.figure.robot);
0513 drawrobot3d(robot, robot.q);
0514 draw_target_points();
0515 
0516 % --- Executes during object creation, after setting all properties.
0517 function slider_q5_CreateFcn(hObject, eventdata, handles)
0518 % hObject    handle to slider_q5 (see GCBO)
0519 % eventdata  reserved - to be defined in a future version of MATLAB
0520 % handles    empty - handles not created until after all CreateFcns called
0521 
0522 
0523 global robot controls
0524 controls.slider_q5=hObject;
0525 if robot.DOF>4
0526     %modify the limits of the slider object to fit max and min limits
0527     set(hObject, 'Min',robot.maxangle(5,1)*180/pi,'Max',robot.maxangle(5,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0528 else 
0529     %make the control disappear
0530      set(hObject, 'Min',0,'Max',0, 'Value',0);
0531     disp('\nPlease set robot.DOF correspondingly')
0532 end
0533 
0534 
0535 % Hint: slider controls usually have a light gray background.
0536 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0537     set(hObject,'BackgroundColor',[.9 .9 .9]);
0538 end
0539 
0540 
0541 % --- Executes on slider movement.
0542 function slider_q6_Callback(hObject, eventdata, handles)
0543 % hObject    handle to slider_q6 (see GCBO)
0544 % eventdata  reserved - to be defined in a future version of MATLAB
0545 % handles    structure with handles and user data (see GUIDATA)
0546 
0547 % Hints: get(hObject,'Value') returns position of slider
0548 %        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
0549 
0550 global configuration robot controls
0551 slider_value = get(hObject,'Value');
0552 set(controls.edit_q6, 'String', num2str(slider_value));
0553 %convert to rads and store
0554 if robot.kind(6)=='R'
0555     robot.q(6) = slider_value*pi/180;%rad
0556 else
0557     robot.q(6) = slider_value;%m
0558 end
0559 update_T_Q();
0560 figure(configuration.figure.robot);
0561 drawrobot3d(robot, robot.q);
0562 draw_target_points();
0563 
0564 
0565 % --- Executes during object creation, after setting all properties.
0566 function slider_q6_CreateFcn(hObject, eventdata, handles)
0567 % hObject    handle to slider_q6 (see GCBO)
0568 % eventdata  reserved - to be defined in a future version of MATLAB
0569 % handles    empty - handles not created until after all CreateFcns called
0570 
0571 
0572 global robot controls
0573 
0574 controls.slider_q6=hObject;
0575 
0576 if robot.DOF>5
0577     %modify the limits of the slider object to fit max and min limits
0578     set(hObject, 'Min',robot.maxangle(6,1)*180/pi,'Max',robot.maxangle(6,2)*180/pi, 'Value',0,'SliderStep',[0.005 0.2]);
0579 else 
0580     %make the control disappear
0581      set(hObject, 'Min',0,'Max',0, 'Value',0);
0582     disp('\nPlease set robot.DOF correspondingly')
0583 end
0584 
0585 
0586 % Hint: slider controls usually have a light gray background.
0587 if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
0588     set(hObject,'BackgroundColor',[.9 .9 .9]);
0589 end
0590 
0591 
0592 
0593 
0594 % --- Executes on button press in pushbutton_x_minus.
0595 function pushbutton_x_minus_Callback(hObject, eventdata, handles)
0596 % hObject    handle to pushbutton_x_minus (see GCBO)
0597 % eventdata  reserved - to be defined in a future version of MATLAB
0598 % handles    structure with handles and user data (see GUIDATA)
0599 
0600 global robot controls
0601 
0602 
0603 %FIND RESOLUTION OF MOVEMENT
0604 resolution=get(controls.popupmenu_resolution,'Value');
0605 if resolution==3 %low
0606     delta=0.05; %m
0607 end
0608 if resolution==2
0609     delta=0.1; %m
0610 end
0611 if resolution==1
0612     delta=0.2; %m
0613 end
0614 
0615 
0616 
0617 T = directkinematic(robot, robot.q);
0618 P_ini=T(1:3,4);
0619 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0620 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0621 
0622 % MOVE IN LINES
0623 if line_reorient==1
0624     base=get(controls.popupmenu_base_end,'Value');
0625     
0626     if base==1 %move in base coordinates
0627         P_final = P_ini;
0628         P_final(1) = P_ini(1) - delta;
0629     else %move in end effector's coordinates
0630         P_final = P_ini - delta*T(1:3,1);
0631     end
0632     
0633     follow_line(P_ini,P_final);
0634     
0635 else% REORIENTATION
0636     
0637     %compute objective T with rotation
0638     Trot = [1    0          0        0;
0639         0 cos(-delta) -sin(-delta) 0;
0640         0  sin(-delta) cos(-delta) 0;
0641         0   0              0     1];
0642     
0643     base=get(controls.popupmenu_base_end,'Value');
0644     
0645     if base==1 %move in base coordinates
0646         T_final = Trot*T;
0647     else %move in end effector's coordinates
0648         T_final = T*Trot;
0649     end
0650     
0651     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
0652     conf=compute_configuration(robot, robot.q);
0653     
0654     
0655     %convert to quaternion
0656     Q = T2quaternion(T_final);
0657     Q=real(Q);
0658     tp=[[P_ini'],[Q],conf,ejes_ext];
0659     
0660     command =[' MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
0661     eval(command);
0662 end
0663 
0664 update_T_Q();
0665 update_sliders();
0666 draw_target_points();
0667 
0668 
0669 % --- Executes on button press in pushbutton_x_plus.
0670 function pushbutton_x_plus_Callback(hObject, eventdata, handles)
0671 % hObject    handle to pushbutton_x_plus (see GCBO)
0672 % eventdata  reserved - to be defined in a future version of MATLAB
0673 % handles    structure with handles and user data (see GUIDATA)
0674 
0675 global robot controls
0676 %substract this quantity to X in base coordinates
0677 %compute total movement
0678 resolution=get(controls.popupmenu_resolution,'Value');
0679 if resolution==3 %low
0680     delta=0.05; %m
0681 end
0682 if resolution==2
0683     delta=0.1; %m
0684 end
0685 if resolution==1
0686     delta=0.2; %m
0687 end
0688 
0689 
0690 T = directkinematic(robot, robot.q);
0691 P_ini=T(1:3,4);
0692 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0693 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0694 
0695 % MOVE IN LINES
0696 if line_reorient==1
0697     base=get(controls.popupmenu_base_end,'Value');
0698     
0699     if base==1 %move in base coordinates
0700         P_final = P_ini;
0701         P_final(1) = P_ini(1) + delta;
0702     else %move in end effector's coordinates
0703         P_final = P_ini + delta*T(1:3,1);
0704     end
0705     
0706     follow_line(P_ini,P_final);
0707     
0708 else% REORIENTATION
0709     
0710     %compute objective T with rotation
0711     Trot = [1    0          0        0;
0712         0 cos(delta) -sin(delta) 0;
0713         0  sin(delta) cos(delta) 0;
0714         0   0              0     1];
0715     
0716     base=get(controls.popupmenu_base_end,'Value');
0717     
0718     if base==1 %move in base coordinates
0719         T_final = Trot*T;
0720     else %move in end effector's coordinates
0721         T_final = T*Trot;
0722     end
0723     
0724     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
0725     conf=compute_configuration(robot, robot.q);
0726     
0727     
0728     %convert to quaternion
0729     Q = T2quaternion(T_final);
0730     Q=real(Q);
0731     tp=[[P_ini'],[Q],conf,ejes_ext];
0732     
0733     command =[' MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
0734     eval(command);
0735 end
0736 
0737 update_T_Q();
0738 update_sliders();
0739 draw_target_points();
0740 
0741 
0742 % --- Executes on button press in pushbutton_y_plus.
0743 function pushbutton_y_plus_Callback(hObject, eventdata, handles)
0744 % hObject    handle to pushbutton_y_plus (see GCBO)
0745 % eventdata  reserved - to be defined in a future version of MATLAB
0746 % handles    structure with handles and user data (see GUIDATA)
0747 
0748 
0749 global robot controls
0750 %substract this quantity to X in base coordinates
0751 %compute total movement
0752 resolution=get(controls.popupmenu_resolution,'Value');
0753 if resolution==3 %low
0754     delta=0.05; %m
0755 end
0756 if resolution==2
0757     delta=0.1; %m
0758 end
0759 if resolution==1
0760     delta=0.2; %m
0761 end
0762 
0763 
0764 T = directkinematic(robot, robot.q);
0765 P_ini=T(1:3,4);
0766 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0767 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0768 
0769 % MOVE IN LINES
0770 if line_reorient==1
0771     base=get(controls.popupmenu_base_end,'Value');
0772     
0773     if base==1 %move in base coordinates
0774         P_final = P_ini;
0775         P_final(2) = P_ini(2) + delta;
0776     else %move in end effector's coordinates
0777         P_final = P_ini + delta*T(1:3,2);
0778     end
0779     
0780     follow_line(P_ini,P_final);
0781     
0782 else% REORIENTATION
0783     
0784     %compute objective T with rotation
0785     Trot = [cos(delta)    0          sin(delta)        0;
0786             0             1            0                0;
0787             -sin(delta)   0          cos(delta)         0;
0788             0             0              0               1];
0789     
0790     base=get(controls.popupmenu_base_end,'Value');
0791     
0792     if base==1 %move in base coordinates
0793         T_final = Trot*T;
0794     else %move in end effector's coordinates
0795         T_final = T*Trot;
0796     end
0797     
0798     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
0799     conf=compute_configuration(robot, robot.q);
0800     
0801     
0802     %convert to quaternion
0803     Q = T2quaternion(T_final);
0804     Q=real(Q);
0805     tp=[[P_ini'],[Q],conf,ejes_ext];
0806     
0807     command =['MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
0808     eval(command);
0809 end
0810 
0811 update_T_Q();
0812 update_sliders();
0813 draw_target_points();
0814 
0815 % --- Executes on button press in pushbutton_y_minus.
0816 function pushbutton_y_minus_Callback(hObject, eventdata, handles)
0817 % hObject    handle to pushbutton_y_minus (see GCBO)
0818 % eventdata  reserved - to be defined in a future version of MATLAB
0819 % handles    structure with handles and user data (see GUIDATA)
0820 global robot controls
0821 %substract this quantity to X in base coordinates
0822 %compute total movement
0823 resolution=get(controls.popupmenu_resolution,'Value');
0824 if resolution==3 %low
0825     delta=0.05; %m
0826 end
0827 if resolution==2
0828     delta=0.1; %m
0829 end
0830 if resolution==1
0831     delta=0.2; %m
0832 end
0833 
0834 
0835 T = directkinematic(robot, robot.q);
0836 P_ini=T(1:3,4);
0837 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0838 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0839 
0840 % MOVE IN LINES
0841 if line_reorient==1
0842     base=get(controls.popupmenu_base_end,'Value');
0843     
0844     if base==1 %move in base coordinates
0845         P_final = P_ini;
0846         P_final(2) = P_ini(2) - delta;
0847     else %move in end effector's coordinates
0848         P_final = P_ini - delta*T(1:3,2);
0849     end
0850     
0851     follow_line(P_ini,P_final);
0852     
0853 else% REORIENTATION
0854     
0855     %compute objective T with rotation
0856     Trot = [cos(-delta)    0          sin(-delta)        0;
0857             0             1            0                0;
0858             -sin(-delta)   0          cos(-delta)         0;
0859             0             0              0               1];
0860     
0861     base=get(controls.popupmenu_base_end,'Value');
0862     
0863     if base==1 %move in base coordinates
0864         T_final = Trot*T;
0865     else %move in end effector's coordinates
0866         T_final = T*Trot;
0867     end
0868     
0869     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
0870     conf=compute_configuration(robot, robot.q);
0871     
0872     
0873     %convert to quaternion
0874     Q = T2quaternion(T_final);
0875     Q=real(Q);
0876     tp=[[P_ini'],[Q],conf,ejes_ext];
0877     
0878     command =['MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
0879     eval(command);
0880 end
0881 
0882 update_T_Q();
0883 update_sliders();
0884 draw_target_points();
0885 
0886 % --- Executes on button press in pushbutton_z_plus.
0887 function pushbutton_z_plus_Callback(hObject, eventdata, handles)
0888 % hObject    handle to pushbutton_z_plus (see GCBO)
0889 % eventdata  reserved - to be defined in a future version of MATLAB
0890 % handles    structure with handles and user data (see GUIDATA)
0891 
0892 global robot controls
0893 %substract this quantity to X in base coordinates
0894 
0895 %compute total movement
0896 resolution=get(controls.popupmenu_resolution,'Value');
0897 if resolution==3 %low
0898     delta=0.05; %m
0899 end
0900 if resolution==2
0901     delta=0.1; %m
0902 end
0903 if resolution==1
0904     delta=0.2; %m
0905 end
0906 
0907 
0908 T = directkinematic(robot, robot.q);
0909 P_ini=T(1:3,4);
0910 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0911 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0912 
0913 % MOVE IN LINES
0914 if line_reorient==1
0915     base=get(controls.popupmenu_base_end,'Value');
0916     
0917     if base==1 %move in base coordinates
0918         P_final = P_ini;
0919         P_final(3) = P_ini(3) + delta;
0920     else %move in end effector's coordinates
0921         P_final = P_ini + delta*T(1:3,3);
0922     end
0923     
0924     follow_line(P_ini,P_final);
0925     
0926 else% REORIENTATION
0927     
0928     %compute objective T with rotation
0929     Trot = [cos(delta)       -sin(delta)          0     0;
0930             sin(delta)         cos(delta)         0     0;
0931                 0                  0              1     0;
0932                 0                   0              0    1];
0933     
0934     base=get(controls.popupmenu_base_end,'Value');
0935     
0936     if base==1 %move in base coordinates
0937         T_final = Trot*T;
0938     else %move in end effector's coordinates
0939         T_final = T*Trot;
0940     end
0941     
0942     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
0943     conf=compute_configuration(robot, robot.q);
0944     
0945     
0946     %convert to quaternion
0947     Q = T2quaternion(T_final);
0948     Q=real(Q);
0949     %Do not remove tp, since it is evaluated
0950     tp=[[P_ini'],[Q],conf,ejes_ext];
0951     MoveJ(tp, 'vmax', 'fine', robot.tool0, robot.wobj0);
0952     %command =[' MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
0953     %eval(command);
0954 end
0955 
0956 update_T_Q();
0957 update_sliders();
0958 draw_target_points();
0959 
0960 % --- Executes on button press in pushbutton_z_minus.
0961 function pushbutton_z_minus_Callback(hObject, eventdata, handles)
0962 % hObject    handle to pushbutton_z_minus (see GCBO)
0963 % eventdata  reserved - to be defined in a future version of MATLAB
0964 % handles    structure with handles and user data (see GUIDATA)
0965 global robot controls
0966 %substract this quantity to X in base coordinates
0967 %compute total movement
0968 resolution=get(controls.popupmenu_resolution,'Value');
0969 if resolution==3 %low
0970     delta=0.05; %m
0971 end
0972 if resolution==2
0973     delta=0.1; %m
0974 end
0975 if resolution==1
0976     delta=0.2; %m
0977 end
0978 
0979 
0980 T = directkinematic(robot, robot.q);
0981 P_ini=T(1:3,4);
0982 % FIND KIND OF MOVEMENT, LINES OR REORIENTATION
0983 line_reorient=get(controls.popupmenu_line_reorient,'Value');
0984 
0985 % MOVE IN LINES
0986 if line_reorient==1
0987     base=get(controls.popupmenu_base_end,'Value');
0988     
0989     if base==1 %move in base coordinates
0990         P_final = P_ini;
0991         P_final(3) = P_ini(3) - delta;
0992     else %move in end effector's coordinates
0993         P_final = P_ini - delta*T(1:3,3);
0994     end
0995     
0996     follow_line(P_ini,P_final);
0997     
0998 else% REORIENTATION
0999     
1000     %compute objective T with rotation
1001     Trot = [cos(-delta)       -sin(-delta)          0     0;
1002             sin(-delta)         cos(-delta)         0     0;
1003                 0                  0              1     0;
1004                 0                   0              0    1];
1005     
1006     base=get(controls.popupmenu_base_end,'Value');
1007     
1008     if base==1 %move in base coordinates
1009         T_final = Trot*T;
1010     else %move in end effector's coordinates
1011         T_final = T*Trot;
1012     end
1013     
1014     ejes_ext = [9E+09,9E+09,9E+09,9E+09,9E+09,9E+09];
1015     conf=compute_configuration(robot, robot.q);
1016     
1017     
1018     %convert to quaternion
1019     Q = T2quaternion(T_final);
1020     Q=real(Q);
1021     tp=[[P_ini'],[Q],conf,ejes_ext];
1022     
1023     command =[' MoveJ(tp,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
1024     eval(command);
1025 end
1026 
1027 update_T_Q();
1028 update_sliders();
1029 draw_target_points();
1030 
1031 
1032 function edit_q1_Callback(hObject, eventdata, handles)
1033 % hObject    handle to edit_q1 (see GCBO)
1034 % eventdata  reserved - to be defined in a future version of MATLAB
1035 % handles    structure with handles and user data (see GUIDATA)
1036 
1037 % Hints: get(hObject,'String') returns contents of edit_q1 as text
1038 %        str2double(get(hObject,'String')) returns contents of edit_q1 as a double
1039 
1040 
1041 % --- Executes during object creation, after setting all properties.
1042 function edit_q1_CreateFcn(hObject, eventdata, handles)
1043 % hObject    handle to edit_q1 (see GCBO)
1044 % eventdata  reserved - to be defined in a future version of MATLAB
1045 % handles    empty - handles not created until after all CreateFcns called
1046 
1047 global controls
1048 
1049 %save the control so that it can be accessed elsewhere
1050 controls.edit_q1=hObject;
1051 
1052 % Hint: edit controls usually have a white background on Windows.
1053 %       See ISPC and COMPUTER.
1054 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1055     set(hObject,'BackgroundColor','white');
1056     
1057 end
1058 
1059 %set(hObject, 'String', 'Push button pushed')
1060 
1061 function edit_q2_Callback(hObject, eventdata, handles)
1062 % hObject    handle to edit_q2 (see GCBO)
1063 % eventdata  reserved - to be defined in a future version of MATLAB
1064 % handles    structure with handles and user data (see GUIDATA)
1065 
1066 % Hints: get(hObject,'String') returns contents of edit_q2 as text
1067 %        str2double(get(hObject,'String')) returns contents of edit_q2 as a double
1068 
1069 
1070 % --- Executes during object creation, after setting all properties.
1071 function edit_q2_CreateFcn(hObject, eventdata, handles)
1072 % hObject    handle to edit_q2 (see GCBO)
1073 % eventdata  reserved - to be defined in a future version of MATLAB
1074 % handles    empty - handles not created until after all CreateFcns called
1075 
1076 global controls
1077 
1078 %save the control so that it can be accessed elsewhere
1079 controls.edit_q2=hObject;
1080 
1081 % Hint: edit controls usually have a white background on Windows.
1082 %       See ISPC and COMPUTER.
1083 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1084     set(hObject,'BackgroundColor','white');
1085 end
1086 
1087 
1088 
1089 function edit_q3_Callback(hObject, eventdata, handles)
1090 % hObject    handle to edit_q3 (see GCBO)
1091 % eventdata  reserved - to be defined in a future version of MATLAB
1092 % handles    structure with handles and user data (see GUIDATA)
1093 
1094 % Hints: get(hObject,'String') returns contents of edit_q3 as text
1095 %        str2double(get(hObject,'String')) returns contents of edit_q3 as a double
1096 
1097 
1098 
1099 % --- Executes during object creation, after setting all properties.
1100 function edit_q3_CreateFcn(hObject, eventdata, handles)
1101 % hObject    handle to edit_q3 (see GCBO)
1102 % eventdata  reserved - to be defined in a future version of MATLAB
1103 % handles    empty - handles not created until after all CreateFcns called
1104 
1105 global controls
1106 
1107 %save the control so that it can be accessed elsewhere
1108 controls.edit_q3=hObject;
1109 
1110 % Hint: edit controls usually have a white background on Windows.
1111 %       See ISPC and COMPUTER.
1112 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1113     set(hObject,'BackgroundColor','white');
1114 end
1115 
1116 
1117 
1118 function edit_q4_Callback(hObject, eventdata, handles)
1119 % hObject    handle to edit_q4 (see GCBO)
1120 % eventdata  reserved - to be defined in a future version of MATLAB
1121 % handles    structure with handles and user data (see GUIDATA)
1122 
1123 % Hints: get(hObject,'String') returns contents of edit_q4 as text
1124 %        str2double(get(hObject,'String')) returns contents of edit_q4 as a double
1125 
1126 
1127 % --- Executes during object creation, after setting all properties.
1128 function edit_q4_CreateFcn(hObject, eventdata, handles)
1129 % hObject    handle to edit_q4 (see GCBO)
1130 % eventdata  reserved - to be defined in a future version of MATLAB
1131 % handles    empty - handles not created until after all CreateFcns called
1132 
1133 
1134 global controls
1135 
1136 %save the control so that it can be accessed elsewhere
1137 controls.edit_q4=hObject;
1138 
1139 % Hint: edit controls usually have a white background on Windows.
1140 %       See ISPC and COMPUTER.
1141 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1142     set(hObject,'BackgroundColor','white');
1143 end
1144 
1145 
1146 
1147 function edit_q5_Callback(hObject, eventdata, handles)
1148 % hObject    handle to edit_q5 (see GCBO)
1149 % eventdata  reserved - to be defined in a future version of MATLAB
1150 % handles    structure with handles and user data (see GUIDATA)
1151 
1152 % Hints: get(hObject,'String') returns contents of edit_q5 as text
1153 %        str2double(get(hObject,'String')) returns contents of edit_q5 as a double
1154 
1155 
1156 % --- Executes during object creation, after setting all properties.
1157 function edit_q5_CreateFcn(hObject, eventdata, handles)
1158 % hObject    handle to edit_q5 (see GCBO)
1159 % eventdata  reserved - to be defined in a future version of MATLAB
1160 % handles    empty - handles not created until after all CreateFcns called
1161 
1162 
1163 global controls
1164 
1165 %save the control so that it can be accessed elsewhere
1166 controls.edit_q5=hObject;
1167 
1168 % Hint: edit controls usually have a white background on Windows.
1169 %       See ISPC and COMPUTER.
1170 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1171     set(hObject,'BackgroundColor','white');
1172 end
1173 
1174 
1175 
1176 function edit_q6_Callback(hObject, eventdata, handles)
1177 % hObject    handle to edit_q6 (see GCBO)
1178 % eventdata  reserved - to be defined in a future version of MATLAB
1179 % handles    structure with handles and user data (see GUIDATA)
1180 
1181 % Hints: get(hObject,'String') returns contents of edit_q6 as text
1182 %        str2double(get(hObject,'String')) returns contents of edit_q6 as a double
1183 
1184 
1185 % --- Executes during object creation, after setting all properties.
1186 function edit_q6_CreateFcn(hObject, eventdata, handles)
1187 % hObject    handle to edit_q6 (see GCBO)
1188 % eventdata  reserved - to be defined in a future version of MATLAB
1189 % handles    empty - handles not created until after all CreateFcns called
1190 
1191 
1192 global controls
1193 
1194 %save the control so that it can be accessed elsewhere
1195 controls.edit_q6=hObject;
1196 
1197 % Hint: edit controls usually have a white background on Windows.
1198 %       See ISPC and COMPUTER.
1199 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1200     set(hObject,'BackgroundColor','white');
1201 end
1202 
1203 
1204 % --- Executes on button press in pushbutton_load.
1205 function pushbutton_load_Callback(hObject, eventdata, handles)
1206 % hObject    handle to pushbutton_load (see GCBO)
1207 % eventdata  reserved - to be defined in a future version of MATLAB
1208 % handles    structure with handles and user data (see GUIDATA)
1209 
1210 global program controls targets robot
1211 
1212 program=[];
1213 
1214 %always require a filename
1215 [filename, pathname] = uigetfile('*.m','Select .m program to load');
1216 if isequal(filename,0)
1217    disp('User selected Cancel')
1218    return;
1219 else
1220    disp(['User selected', fullfile(pathname, filename)])
1221 end
1222 
1223 
1224 %open selected file for read only
1225 fp=fopen(fullfile(pathname, filename),'r');
1226 
1227 disp('Loading target points');
1228 while 1
1229     a = fgets(fp);
1230     k = strfind(a, '%BEGINTARGETPOINTS');
1231    
1232     %find beginning of target points
1233     if k>=0
1234         break;
1235     end
1236 end
1237 
1238 target_names=[];
1239 %reset number of target points
1240 N=0;
1241 %now load target points
1242 while 1
1243     a = fgets(fp);
1244     if length(a)<=1
1245         continue;
1246     end
1247     k = strfind(a, '%ENDTARGETPOINTS');
1248     %test whether there are no more target points
1249     if k>=0
1250         break;
1251     end
1252     
1253     %test whether its a valid target point initialization in matlab
1254     k = strfind(a, ':=');
1255     if k>=0
1256         continue;
1257     end
1258 
1259     [name, Q, T, conf]=get_targetpoint_from_string(a);
1260         
1261     N=N+1;
1262     targets{N}.name=name;
1263     targets{N}.Q=Q;
1264     targets{N}.T=T;
1265     targets{N}.conf=conf;
1266     
1267     
1268     qinv = inversekinematic(robot, T);
1269     q = select_configuration(robot, qinv, conf);
1270     targets{N}.q=q;
1271     
1272     target_names{N}=name;
1273     
1274     %store the name in the programming popup storing the available targets.
1275     set(controls.popupmenu_target_point,'String',target_names);  
1276     set(controls.popupmenu_through_point,'String',target_names); 
1277     
1278 end
1279 controls.num_target_points=N;
1280 
1281 disp('Loading program');
1282 program.n_lines=0;
1283 %now load program
1284 target_names=[];
1285 %reset number of target points
1286 N=0;
1287 %now load target points
1288 while 1
1289     a = fgets(fp);
1290     if length(a)<=1
1291         continue;
1292     end
1293     
1294     k = strfind(a, '%ENDMODULE');
1295     %test whether we reached the end of the program
1296     if k>=0
1297         break;
1298     end
1299     
1300     %test whether its a valid target point initialization in matlab
1301     k = strfind(a, '(');
1302     if isempty(k)
1303         continue;
1304     end
1305 
1306     N = N+1;
1307     
1308     program.n_lines=N;
1309     program.matlab_lines{N}=a(1:end-1); %remove carriage return
1310     %program.rapid_lines{N}=a;
1311 end
1312 
1313 fclose(fp);
1314 
1315 draw_target_points();
1316 
1317 
1318 %Save the whole program to a .m file
1319 % --- Executes on button press in pushbutton_save.
1320 function pushbutton_save_Callback(hObject, eventdata, handles)
1321 % hObject    handle to pushbutton_save (see GCBO)
1322 % eventdata  reserved - to be defined in a future version of MATLAB
1323 % handles    structure with handles and user data (see GUIDATA)
1324 
1325 global program controls targets
1326 
1327 %always require a filename
1328 [filename, pathname] = uiputfile('*.m','Save as .m');
1329 if isequal(filename,0) || isequal(pathname,0)
1330    disp('User selected Cancel')
1331    return;
1332 else
1333    disp(['User selected',fullfile(pathname,filename)])
1334 end
1335 
1336 
1337 %open selected file
1338 fp=fopen(fullfile(pathname, filename),'w');
1339 
1340 disp('Saving target points');
1341 
1342 fprintf(fp, '%%BEGINMODULE');
1343 %write two escape characters to
1344 fprintf(fp, '\n%%BEGINTARGETPOINTS');
1345 for i=1:controls.num_target_points,
1346     tp = construct_target(i);
1347     ejes_ext = '[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];';
1348     a_rap = sprintf('%s:=[[%.5f, %.5f, %.5f],[%.5f, %.5f, %.5f, %.5f], [%.5f, %.5f, %.5f, %.5f], %s',...
1349     targets{i}.name, targets{i}.T(1,4),targets{i}.T(2,4),targets{i}.T(3,4),...
1350             targets{i}.Q(1), targets{i}.Q(2), targets{i}.Q(3), targets{i}.Q(4),...
1351         targets{i}.conf(1), targets{i}.conf(2), targets{i}.conf(3), targets{i}.conf(4), ejes_ext);
1352     a_mat = sprintf('%s=[[%.5f, %.5f, %.5f],[%.5f, %.5f, %.5f, %.5f], [%.5f, %.5f, %.5f, %.5f], %s',...
1353     targets{i}.name, targets{i}.T(1,4),targets{i}.T(2,4),targets{i}.T(3,4),...
1354             targets{i}.Q(1), targets{i}.Q(2), targets{i}.Q(3), targets{i}.Q(4),...
1355         targets{i}.conf(1), targets{i}.conf(2), targets{i}.conf(3), targets{i}.conf(4), ejes_ext);
1356     %write file
1357     %fprintf(fp, '\n%%%s', a_rap);
1358     fprintf(fp, '\n%s', a_mat);
1359 end
1360 
1361 fprintf(fp, '\n%%ENDTARGETPOINTS\n');
1362 
1363 
1364 disp('Saving program lines');
1365 for i=1:program.n_lines,
1366     fprintf(fp, '\n%s', program.matlab_lines{i});
1367    % fprintf(fp, '\n%%%s', program.rapid_lines{i});
1368 end
1369 
1370 fprintf(fp, '\n%%ENDMODULE\n');
1371 fclose(fp);
1372 
1373 
1374 % simulate the whole program
1375 % --- Executes on button press in pushbutton_simulate.
1376 function pushbutton_simulate_Callback(hObject, eventdata, handles)
1377 % hObject    handle to pushbutton_simulate (see GCBO)
1378 % eventdata  reserved - to be defined in a future version of MATLAB
1379 % handles    structure with handles and user data (see GUIDATA)
1380 
1381 global program targets controls robot
1382 
1383 %reset position velocity acceleration and time
1384 robot.q_vector=[];
1385 robot.qd_vector=[];
1386 robot.qdd_vector=[];
1387 robot.time=[];
1388 
1389 
1390 ejes_ext = '[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];';
1391 
1392 for i=1:controls.num_target_points,
1393     a_mat = sprintf('%s=[[%.5f, %.5f, %.5f],[%.5f, %.5f, %.5f, %.5f], [%.5f, %.5f, %.5f, %.5f], %s',...
1394     targets{i}.name, targets{i}.T(1,4),targets{i}.T(2,4),targets{i}.T(3,4),...
1395     targets{i}.Q(1), targets{i}.Q(2), targets{i}.Q(3), targets{i}.Q(4),...
1396     targets{i}.conf(1), targets{i}.conf(2), targets{i}.conf(3), targets{i}.conf(4), ejes_ext);
1397     eval(a_mat);
1398 end
1399 
1400 for i=1:program.n_lines,
1401     eval(program.matlab_lines{i});
1402     update_T_Q();
1403     update_sliders();
1404     draw_target_points();
1405 end
1406 
1407 robot.q_vector=[];
1408 robot.qd_vector=[];
1409 robot.qdd_vector=[];
1410 robot.time=[];
1411 
1412 
1413 
1414 function edit_nx_Callback(hObject, eventdata, handles)
1415 % hObject    handle to edit_nx (see GCBO)
1416 % eventdata  reserved - to be defined in a future version of MATLAB
1417 % handles    structure with handles and user data (see GUIDATA)
1418 
1419 % Hints: get(hObject,'String') returns contents of edit_nx as text
1420 %        str2double(get(hObject,'String')) returns contents of edit_nx as a double
1421 
1422 
1423 % --- Executes during object creation, after setting all properties.
1424 function edit_nx_CreateFcn(hObject, eventdata, handles)
1425 % hObject    handle to edit_nx (see GCBO)
1426 % eventdata  reserved - to be defined in a future version of MATLAB
1427 % handles    empty - handles not created until after all CreateFcns called
1428 global controls
1429 controls.edit_nx=hObject;
1430 % Hint: edit controls usually have a white background on Windows.
1431 %       See ISPC and COMPUTER.
1432 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1433     set(hObject,'BackgroundColor','white');
1434 end
1435 
1436 
1437 
1438 function edit_ox_Callback(hObject, eventdata, handles)
1439 % hObject    handle to edit_ox (see GCBO)
1440 % eventdata  reserved - to be defined in a future version of MATLAB
1441 % handles    structure with handles and user data (see GUIDATA)
1442 
1443 % Hints: get(hObject,'String') returns contents of edit_ox as text
1444 %        str2double(get(hObject,'String')) returns contents of edit_ox as a double
1445 
1446 
1447 % --- Executes during object creation, after setting all properties.
1448 function edit_ox_CreateFcn(hObject, eventdata, handles)
1449 % hObject    handle to edit_ox (see GCBO)
1450 % eventdata  reserved - to be defined in a future version of MATLAB
1451 % handles    empty - handles not created until after all CreateFcns called
1452 global controls
1453 controls.edit_ox=hObject;
1454 % Hint: edit controls usually have a white background on Windows.
1455 %       See ISPC and COMPUTER.
1456 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1457     set(hObject,'BackgroundColor','white');
1458 end
1459 
1460 
1461 
1462 function edit_ax_Callback(hObject, eventdata, handles)
1463 % hObject    handle to edit_ax (see GCBO)
1464 % eventdata  reserved - to be defined in a future version of MATLAB
1465 % handles    structure with handles and user data (see GUIDATA)
1466 
1467 % Hints: get(hObject,'String') returns contents of edit_ax as text
1468 %        str2double(get(hObject,'String')) returns contents of edit_ax as a double
1469 
1470 
1471 % --- Executes during object creation, after setting all properties.
1472 function edit_ax_CreateFcn(hObject, eventdata, handles)
1473 % hObject    handle to edit_ax (see GCBO)
1474 % eventdata  reserved - to be defined in a future version of MATLAB
1475 % handles    empty - handles not created until after all CreateFcns called
1476 global controls
1477 controls.edit_ax=hObject;
1478 % Hint: edit controls usually have a white background on Windows.
1479 %       See ISPC and COMPUTER.
1480 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1481     set(hObject,'BackgroundColor','white');
1482 end
1483 
1484 
1485 
1486 function edit_pxt_Callback(hObject, eventdata, handles)
1487 % hObject    handle to edit_pxt (see GCBO)
1488 % eventdata  reserved - to be defined in a future version of MATLAB
1489 % handles    structure with handles and user data (see GUIDATA)
1490 
1491 % Hints: get(hObject,'String') returns contents of edit_pxt as text
1492 %        str2double(get(hObject,'String')) returns contents of edit_pxt as a double
1493 
1494 
1495 % --- Executes during object creation, after setting all properties.
1496 function edit_pxt_CreateFcn(hObject, eventdata, handles)
1497 % hObject    handle to edit_pxt (see GCBO)
1498 % eventdata  reserved - to be defined in a future version of MATLAB
1499 % handles    empty - handles not created until after all CreateFcns called
1500 global controls
1501 controls.edit_pxt=hObject;
1502 % Hint: edit controls usually have a white background on Windows.
1503 %       See ISPC and COMPUTER.
1504 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1505     set(hObject,'BackgroundColor','white');
1506 end
1507 
1508 
1509 
1510 function edit_ny_Callback(hObject, eventdata, handles)
1511 % hObject    handle to edit_ny (see GCBO)
1512 % eventdata  reserved - to be defined in a future version of MATLAB
1513 % handles    structure with handles and user data (see GUIDATA)
1514 
1515 % Hints: get(hObject,'String') returns contents of edit_ny as text
1516 %        str2double(get(hObject,'String')) returns contents of edit_ny as a double
1517 
1518 
1519 % --- Executes during object creation, after setting all properties.
1520 function edit_ny_CreateFcn(hObject, eventdata, handles)
1521 % hObject    handle to edit_ny (see GCBO)
1522 % eventdata  reserved - to be defined in a future version of MATLAB
1523 % handles    empty - handles not created until after all CreateFcns called
1524 global controls
1525 controls.edit_ny=hObject;
1526 % Hint: edit controls usually have a white background on Windows.
1527 %       See ISPC and COMPUTER.
1528 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1529     set(hObject,'BackgroundColor','white');
1530 end
1531 
1532 
1533 
1534 function edit_oy_Callback(hObject, eventdata, handles)
1535 % hObject    handle to edit_oy (see GCBO)
1536 % eventdata  reserved - to be defined in a future version of MATLAB
1537 % handles    structure with handles and user data (see GUIDATA)
1538 
1539 % Hints: get(hObject,'String') returns contents of edit_oy as text
1540 %        str2double(get(hObject,'String')) returns contents of edit_oy as a double
1541 
1542 
1543 % --- Executes during object creation, after setting all properties.
1544 function edit_oy_CreateFcn(hObject, eventdata, handles)
1545 % hObject    handle to edit_oy (see GCBO)
1546 % eventdata  reserved - to be defined in a future version of MATLAB
1547 % handles    empty - handles not created until after all CreateFcns called
1548 global controls
1549 controls.edit_oy=hObject;
1550 % Hint: edit controls usually have a white background on Windows.
1551 %       See ISPC and COMPUTER.
1552 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1553     set(hObject,'BackgroundColor','white');
1554 end
1555 
1556 
1557 
1558 function edit_ay_Callback(hObject, eventdata, handles)
1559 % hObject    handle to edit_ay (see GCBO)
1560 % eventdata  reserved - to be defined in a future version of MATLAB
1561 % handles    structure with handles and user data (see GUIDATA)
1562 
1563 % Hints: get(hObject,'String') returns contents of edit_ay as text
1564 %        str2double(get(hObject,'String')) returns contents of edit_ay as a double
1565 
1566 
1567 % --- Executes during object creation, after setting all properties.
1568 function edit_ay_CreateFcn(hObject, eventdata, handles)
1569 % hObject    handle to edit_ay (see GCBO)
1570 % eventdata  reserved - to be defined in a future version of MATLAB
1571 % handles    empty - handles not created until after all CreateFcns called
1572 global controls
1573 controls.edit_ay=hObject;
1574 % Hint: edit controls usually have a white background on Windows.
1575 %       See ISPC and COMPUTER.
1576 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1577     set(hObject,'BackgroundColor','white');
1578 end
1579 
1580 
1581 
1582 function edit_pyt_Callback(hObject, eventdata, handles)
1583 % hObject    handle to edit_pyt (see GCBO)
1584 % eventdata  reserved - to be defined in a future version of MATLAB
1585 % handles    structure with handles and user data (see GUIDATA)
1586 
1587 % Hints: get(hObject,'String') returns contents of edit_pyt as text
1588 %        str2double(get(hObject,'String')) returns contents of edit_pyt as a double
1589 
1590 
1591 % --- Executes during object creation, after setting all properties.
1592 function edit_pyt_CreateFcn(hObject, eventdata, handles)
1593 % hObject    handle to edit_pyt (see GCBO)
1594 % eventdata  reserved - to be defined in a future version of MATLAB
1595 % handles    empty - handles not created until after all CreateFcns called
1596 global controls
1597 controls.edit_pyt=hObject;
1598 % Hint: edit controls usually have a white background on Windows.
1599 %       See ISPC and COMPUTER.
1600 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1601     set(hObject,'BackgroundColor','white');
1602 end
1603 
1604 
1605 
1606 function edit_nz_Callback(hObject, eventdata, handles)
1607 % hObject    handle to edit_nz (see GCBO)
1608 % eventdata  reserved - to be defined in a future version of MATLAB
1609 % handles    structure with handles and user data (see GUIDATA)
1610 
1611 % Hints: get(hObject,'String') returns contents of edit_nz as text
1612 %        str2double(get(hObject,'String')) returns contents of edit_nz as a double
1613 
1614 
1615 % --- Executes during object creation, after setting all properties.
1616 function edit_nz_CreateFcn(hObject, eventdata, handles)
1617 % hObject    handle to edit_nz (see GCBO)
1618 % eventdata  reserved - to be defined in a future version of MATLAB
1619 % handles    empty - handles not created until after all CreateFcns called
1620 global controls
1621 controls.edit_nz=hObject;
1622 % Hint: edit controls usually have a white background on Windows.
1623 %       See ISPC and COMPUTER.
1624 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1625     set(hObject,'BackgroundColor','white');
1626 end
1627 
1628 
1629 
1630 function edit_oz_Callback(hObject, eventdata, handles)
1631 % hObject    handle to edit_oz (see GCBO)
1632 % eventdata  reserved - to be defined in a future version of MATLAB
1633 % handles    structure with handles and user data (see GUIDATA)
1634 
1635 % Hints: get(hObject,'String') returns contents of edit_oz as text
1636 %        str2double(get(hObject,'String')) returns contents of edit_oz as a double
1637 
1638 
1639 % --- Executes during object creation, after setting all properties.
1640 function edit_oz_CreateFcn(hObject, eventdata, handles)
1641 % hObject    handle to edit_oz (see GCBO)
1642 % eventdata  reserved - to be defined in a future version of MATLAB
1643 % handles    empty - handles not created until after all CreateFcns called
1644 global controls
1645 controls.edit_oz=hObject;
1646 % Hint: edit controls usually have a white background on Windows.
1647 %       See ISPC and COMPUTER.
1648 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1649     set(hObject,'BackgroundColor','white');
1650 end
1651 
1652 
1653 
1654 function edit_az_Callback(hObject, eventdata, handles)
1655 % hObject    handle to edit_az (see GCBO)
1656 % eventdata  reserved - to be defined in a future version of MATLAB
1657 % handles    structure with handles and user data (see GUIDATA)
1658 
1659 % Hints: get(hObject,'String') returns contents of edit_az as text
1660 %        str2double(get(hObject,'String')) returns contents of edit_az as a double
1661 
1662 
1663 % --- Executes during object creation, after setting all properties.
1664 function edit_az_CreateFcn(hObject, eventdata, handles)
1665 % hObject    handle to edit_az (see GCBO)
1666 % eventdata  reserved - to be defined in a future version of MATLAB
1667 % handles    empty - handles not created until after all CreateFcns called
1668 global controls
1669 controls.edit_az=hObject;
1670 % Hint: edit controls usually have a white background on Windows.
1671 %       See ISPC and COMPUTER.
1672 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1673     set(hObject,'BackgroundColor','white');
1674 end
1675 
1676 
1677 
1678 function edit_pzt_Callback(hObject, eventdata, handles)
1679 % hObject    handle to edit_pzt (see GCBO)
1680 % eventdata  reserved - to be defined in a future version of MATLAB
1681 % handles    structure with handles and user data (see GUIDATA)
1682 
1683 % Hints: get(hObject,'String') returns contents of edit_pzt as text
1684 %        str2double(get(hObject,'String')) returns contents of edit_pzt as a double
1685 
1686 
1687 % --- Executes during object creation, after setting all properties.
1688 function edit_pzt_CreateFcn(hObject, eventdata, handles)
1689 % hObject    handle to edit_pzt (see GCBO)
1690 % eventdata  reserved - to be defined in a future version of MATLAB
1691 % handles    empty - handles not created until after all CreateFcns called
1692 global controls
1693 controls.edit_pzt=hObject;
1694 % Hint: edit controls usually have a white background on Windows.
1695 %       See ISPC and COMPUTER.
1696 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1697     set(hObject,'BackgroundColor','white');
1698 end
1699 
1700 
1701 % SOLVE THE INVERSE KINEMATIC PROBLEM AND GO TO IT
1702 %   THE CLOSEST CONFIGURATION IS ALWAYS FOUND IN THE JOINT SPACE
1703 % --- Executes on button press in pushbutton_moveT.
1704 function pushbutton_moveT_Callback(hObject, eventdata, handles)
1705 % hObject    handle to pushbutton_moveT (see GCBO)
1706 % eventdata  reserved - to be defined in a future version of MATLAB
1707 % handles    structure with handles and user data (see GUIDATA)
1708 
1709 
1710 global robot controls configuration
1711 
1712 %get values of T from dialog
1713 [T,Q,P]=get_TQ_from_dialog();
1714 
1715 %if there is a tool attached to it, the total transformation has
1716 % already considered it in the computation of direct kinematics
1717 % reverse this transformation prior to the call to the inverse kinematics
1718 % function
1719 if isfield(robot, 'tool')
1720     %T=T*inv(robot.tool.TCP);
1721     T=T/(robot.tool.TCP); 
1722 end
1723 
1724 
1725 %several solutions are provided
1726 qinv = inversekinematic(robot, T);
1727 
1728 distance_joint=[];
1729 for i=1:size(qinv,2),
1730    distance_joint(i)=sum(abs(qinv(:,i)-robot.q(:))); 
1731 end
1732 [val,i]=min(distance_joint);
1733 
1734 q_current=robot.q(:);
1735 q_final = qinv(:,i);
1736 
1737 path = lineal_path_plan(q_current, q_final);
1738 
1739 robot.q_vector=path;
1740 %Test whether there are joints outside mechanical limits
1741 test_joint_limits(robot);
1742 
1743 %do not move if current and final coordinates are the same
1744 for i=1:size(path,2),
1745    drawrobot3d(robot, path(:,i)); 
1746    pause(configuration.time_delay);  
1747 end
1748 
1749 robot.q=q_final;
1750 update_T_Q();
1751 update_sliders();
1752 draw_target_points()
1753 
1754 function edit_Q0_Callback(hObject, eventdata, handles)
1755 % hObject    handle to edit_Q0 (see GCBO)
1756 % eventdata  reserved - to be defined in a future version of MATLAB
1757 % handles    structure with handles and user data (see GUIDATA)
1758 
1759 % Hints: get(hObject,'String') returns contents of edit_Q0 as text
1760 %        str2double(get(hObject,'String')) returns contents of edit_Q0 as a double
1761 
1762 
1763 % --- Executes during object creation, after setting all properties.
1764 function edit_Q0_CreateFcn(hObject, eventdata, handles)
1765 % hObject    handle to edit_Q0 (see GCBO)
1766 % eventdata  reserved - to be defined in a future version of MATLAB
1767 % handles    empty - handles not created until after all CreateFcns called
1768 global controls
1769 controls.edit_Q0=hObject;
1770 % Hint: edit controls usually have a white background on Windows.
1771 %       See ISPC and COMPUTER.
1772 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1773     set(hObject,'BackgroundColor','white');
1774 end
1775 
1776 
1777 
1778 function edit_Q1_Callback(hObject, eventdata, handles)
1779 % hObject    handle to edit_Q1 (see GCBO)
1780 % eventdata  reserved - to be defined in a future version of MATLAB
1781 % handles    structure with handles and user data (see GUIDATA)
1782 
1783 % Hints: get(hObject,'String') returns contents of edit_Q1 as text
1784 %        str2double(get(hObject,'String')) returns contents of edit_Q1 as a double
1785 
1786 
1787 % --- Executes during object creation, after setting all properties.
1788 function edit_Q1_CreateFcn(hObject, eventdata, handles)
1789 % hObject    handle to edit_Q1 (see GCBO)
1790 % eventdata  reserved - to be defined in a future version of MATLAB
1791 % handles    empty - handles not created until after all CreateFcns called
1792 global controls
1793 controls.edit_Q1=hObject;
1794 % Hint: edit controls usually have a white background on Windows.
1795 %       See ISPC and COMPUTER.
1796 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1797     set(hObject,'BackgroundColor','white');
1798 end
1799 
1800 
1801 
1802 function edit_Q2_Callback(hObject, eventdata, handles)
1803 % hObject    handle to edit_Q2 (see GCBO)
1804 % eventdata  reserved - to be defined in a future version of MATLAB
1805 % handles    structure with handles and user data (see GUIDATA)
1806 
1807 % Hints: get(hObject,'String') returns contents of edit_Q2 as text
1808 %        str2double(get(hObject,'String')) returns contents of edit_Q2 as a double
1809 
1810 
1811 % --- Executes during object creation, after setting all properties.
1812 function edit_Q2_CreateFcn(hObject, eventdata, handles)
1813 % hObject    handle to edit_Q2 (see GCBO)
1814 % eventdata  reserved - to be defined in a future version of MATLAB
1815 % handles    empty - handles not created until after all CreateFcns called
1816 global controls
1817 controls.edit_Q2=hObject;
1818 % Hint: edit controls usually have a white background on Windows.
1819 %       See ISPC and COMPUTER.
1820 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1821     set(hObject,'BackgroundColor','white');
1822 end
1823 
1824 
1825 
1826 function edit_Q3_Callback(hObject, eventdata, handles)
1827 % hObject    handle to edit_Q3 (see GCBO)
1828 % eventdata  reserved - to be defined in a future version of MATLAB
1829 % handles    structure with handles and user data (see GUIDATA)
1830 
1831 % Hints: get(hObject,'String') returns contents of edit_Q3 as text
1832 %        str2double(get(hObject,'String')) returns contents of edit_Q3 as a double
1833 
1834 
1835 % --- Executes during object creation, after setting all properties.
1836 function edit_Q3_CreateFcn(hObject, eventdata, handles)
1837 % hObject    handle to edit_Q3 (see GCBO)
1838 % eventdata  reserved - to be defined in a future version of MATLAB
1839 % handles    empty - handles not created until after all CreateFcns called
1840 global controls
1841 controls.edit_Q3=hObject;
1842 % Hint: edit controls usually have a white background on Windows.
1843 %       See ISPC and COMPUTER.
1844 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1845     set(hObject,'BackgroundColor','white');
1846 end
1847 
1848 
1849 
1850 function edit_pxq_Callback(hObject, eventdata, handles)
1851 % hObject    handle to edit_pxq (see GCBO)
1852 % eventdata  reserved - to be defined in a future version of MATLAB
1853 % handles    structure with handles and user data (see GUIDATA)
1854 
1855 % Hints: get(hObject,'String') returns contents of edit_pxq as text
1856 %        str2double(get(hObject,'String')) returns contents of edit_pxq as a double
1857 
1858 
1859 % --- Executes during object creation, after setting all properties.
1860 function edit_pxq_CreateFcn(hObject, eventdata, handles)
1861 % hObject    handle to edit_pxq (see GCBO)
1862 % eventdata  reserved - to be defined in a future version of MATLAB
1863 % handles    empty - handles not created until after all CreateFcns called
1864 global controls
1865 controls.edit_pxq=hObject;
1866 % Hint: edit controls usually have a white background on Windows.
1867 %       See ISPC and COMPUTER.
1868 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1869     set(hObject,'BackgroundColor','white');
1870 end
1871 
1872 
1873 
1874 function edit_pyq_Callback(hObject, eventdata, handles)
1875 % hObject    handle to edit_pyq (see GCBO)
1876 % eventdata  reserved - to be defined in a future version of MATLAB
1877 % handles    structure with handles and user data (see GUIDATA)
1878 
1879 % Hints: get(hObject,'String') returns contents of edit_pyq as text
1880 %        str2double(get(hObject,'String')) returns contents of edit_pyq as a double
1881 
1882 
1883 % --- Executes during object creation, after setting all properties.
1884 function edit_pyq_CreateFcn(hObject, eventdata, handles)
1885 % hObject    handle to edit_pyq (see GCBO)
1886 % eventdata  reserved - to be defined in a future version of MATLAB
1887 % handles    empty - handles not created until after all CreateFcns called
1888 global controls
1889 controls.edit_pyq=hObject;
1890 % Hint: edit controls usually have a white background on Windows.
1891 %       See ISPC and COMPUTER.
1892 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1893     set(hObject,'BackgroundColor','white');
1894 end
1895 
1896 
1897 
1898 function edit_pzq_Callback(hObject, eventdata, handles)
1899 % hObject    handle to edit_pzq (see GCBO)
1900 % eventdata  reserved - to be defined in a future version of MATLAB
1901 % handles    structure with handles and user data (see GUIDATA)
1902 
1903 % Hints: get(hObject,'String') returns contents of edit_pzq as text
1904 %        str2double(get(hObject,'String')) returns contents of edit_pzq as a double
1905 
1906 
1907 % --- Executes during object creation, after setting all properties.
1908 function edit_pzq_CreateFcn(hObject, eventdata, handles)
1909 % hObject    handle to edit_pzq (see GCBO)
1910 % eventdata  reserved - to be defined in a future version of MATLAB
1911 % handles    empty - handles not created until after all CreateFcns called
1912 global controls
1913 controls.edit_pzq=hObject;
1914 % Hint: edit controls usually have a white background on Windows.
1915 %       See ISPC and COMPUTER.
1916 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
1917     set(hObject,'BackgroundColor','white');
1918 end
1919 
1920 
1921 
1922 %when pressed, OBTAIN Q from dialog, convert to homogeneous matrix and
1923 %plan a coordinated path to it
1924 % --- Executes on button press in pushbutton_moveQ.
1925 function pushbutton_moveQ_Callback(hObject, eventdata, handles)
1926 % hObject    handle to pushbutton_moveQ (see GCBO)
1927 % eventdata  reserved - to be defined in a future version of MATLAB
1928 % handles    structure with handles and user data (see GUIDATA)
1929 
1930 
1931 global robot controls configuration
1932 
1933 %get values of T from dialog
1934 [T,Q,P]=get_TQ_from_dialog();
1935 
1936 %convert quaternion to orientation
1937 T = quaternion2T(Q);
1938 
1939 T(1,4)=P(1);
1940 T(2,4)=P(2);
1941 T(3,4)=P(3);
1942 
1943 %if there is a tool attached to it, the total transformation has
1944 % already considered it in the computation of direct kinematics
1945 % reverse this transformation prior to the call to the inverse kinematics
1946 % function
1947 if isfield(robot, 'tool')
1948     T=T*inv(robot.tool.TCP); 
1949 end
1950 
1951 
1952 
1953 %several solutions are provided
1954 qinv = inversekinematic(robot, T);
1955 
1956 distance_joint=[];
1957 for i=1:size(qinv,2),
1958    distance_joint(i)=sum(abs(qinv(:,i)-robot.q(:))); 
1959 end
1960 [val,i]=min(distance_joint);
1961 
1962 q_current=robot.q;
1963 q_final = qinv(:,i);
1964 
1965 path = lineal_path_plan(q_current, q_final);
1966 
1967 robot.q_vector=path;
1968 %Test whether there are joints outside mechanical limits
1969 test_joint_limits(robot);
1970 
1971 %do not move if current and final coordinates are the same
1972 for i=1:size(path,2),
1973     drawrobot3d(robot, path(:,i)); 
1974     pause(configuration.time_delay);  
1975 end
1976 
1977 robot.q=q_final;
1978 update_T_Q();
1979 update_sliders();
1980 draw_target_points()
1981 
1982 
1983 
1984 %save a target point to memory
1985 % --- Executes on button press in pushbutton_save_target_point.
1986 function pushbutton_save_target_point_Callback(hObject, eventdata, handles)
1987 % hObject    handle to pushbutton_save_target_point (see GCBO)
1988 % eventdata  reserved - to be defined in a future version of MATLAB
1989 % handles    structure with handles and user data (see GUIDATA)
1990 global controls robot targets
1991 
1992 target_point_name = get(controls.edit_target_point,'String');
1993 
1994 %add a target point
1995 controls.num_target_points = controls.num_target_points+1;
1996 
1997 T = directkinematic(robot, robot.q);
1998 
1999 %if there is a tool attached to it, consider it in the computation of
2000 % direct kinematics
2001 if isfield(robot, 'tool')
2002     T=T*robot.tool.TCP; 
2003 end
2004 
2005 %convert to quaternion
2006 Q = T2quaternion(T);
2007 Q=real(Q);
2008 target.T = T;
2009 target.Q = Q;
2010 target.q = robot.q;%store the current coordinates
2011 target.conf = compute_configuration(robot, robot.q);
2012 target.name = target_point_name;
2013 
2014 
2015 targets{controls.num_target_points}=target;
2016 draw_target_points();
2017 
2018 s0=get(controls.popupmenu_target_point,'String');
2019 a=length(s0);
2020 s0{a+1}=target_point_name;
2021 
2022 %store the name in the programming popup storing the available targets.
2023 set(controls.popupmenu_target_point,'String',s0);
2024 
2025 
2026 %the through point popupmenu
2027 %s0=get(controls.popupmenu_through_point,'String');
2028 %a=length(s0);
2029 %s0{a+1}=target_point_name;
2030 
2031 %store the name in the programming popup storing the available targets.
2032 %set(controls.popupmenu_through_point,'String',s0);
2033 
2034 
2035 
2036 function edit_target_point_Callback(hObject, eventdata, handles)
2037 % hObject    handle to edit_target_point (see GCBO)
2038 % eventdata  reserved - to be defined in a future version of MATLAB
2039 % handles    structure with handles and user data (see GUIDATA)
2040 
2041 % Hints: get(hObject,'String') returns contents of edit_target_point as text
2042 %        str2double(get(hObject,'String')) returns contents of edit_target_point as a double
2043 
2044 
2045 % --- Executes during object creation, after setting all properties.
2046 function edit_target_point_CreateFcn(hObject, eventdata, handles)
2047 % hObject    handle to edit_target_point (see GCBO)
2048 % eventdata  reserved - to be defined in a future version of MATLAB
2049 % handles    empty - handles not created until after all CreateFcns called
2050 global controls
2051 controls.edit_target_point = hObject;
2052 
2053 % Hint: edit controls usually have a white background on Windows.
2054 %       See ISPC and COMPUTER.
2055 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
2056     set(hObject,'BackgroundColor','white');
2057 end
2058 
2059 
2060 % --- Executes on selection change in popupmenu6.
2061 function popupmenu6_Callback(hObject, eventdata, handles)
2062 % hObject    handle to popupmenu6 (see GCBO)
2063 % eventdata  reserved - to be defined in a future version of MATLAB
2064 % handles    structure with handles and user data (see GUIDATA)
2065 
2066 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu6 contents as cell array
2067 %        contents{get(hObject,'Value')} returns selected item from popupmenu6
2068 
2069 
2070 % --- Executes during object creation, after setting all properties.
2071 function popupmenu6_CreateFcn(hObject, eventdata, handles)
2072 % hObject    handle to popupmenu6 (see GCBO)
2073 % eventdata  reserved - to be defined in a future version of MATLAB
2074 % handles    empty - handles not created until after all CreateFcns called
2075 
2076 % Hint: popupmenu controls usually have a white background on Windows.
2077 %       See ISPC and COMPUTER.
2078 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
2079     set(hObject,'BackgroundColor','white');
2080 end
2081 
2082 
2083 % --- Executes on button press in pushbutton_simulate_instruction.
2084 % function pushbutton_simulate_instruction_Callback(hObject, eventdata, handles)
2085 % % hObject    handle to pushbutton_simulate_instruction (see GCBO)
2086 % % eventdata  reserved - to be defined in a future version of MATLAB
2087 % % handles    structure with handles and user data (see GUIDATA)
2088 %
2089 % global controls targets robot
2090 %
2091 % %reset position velocity acceleration and time
2092 % robot.q_vector=[];
2093 % robot.qd_vector=[];
2094 % robot.qdd_vector=[];
2095 % robot.time=[];
2096 %
2097 % tool0=[];
2098 % wobj0=[];
2099 %
2100 % [instruction_matlab, instruction_rapid, tp1, tp2, joint_coord]=build_instruction_to_eval();
2101 % eval(instruction_matlab);
2102 % s = get(controls.popupmenu_instruction,'String');
2103 % i = get(controls.popupmenu_instruction,'Value');
2104 % s1=s{i};
2105 %
2106 % switch s1
2107 %     case {'MoveJ','MoveL'}
2108 %
2109 %         s = get(controls.popupmenu_target_point,'String');
2110 %         target_num = get(controls.popupmenu_target_point,'Value');
2111 %         s2=s{target_num};
2112 %
2113 %         s = get(controls.popupmenu_speed,'String');
2114 %         i = get(controls.popupmenu_speed,'Value');
2115 %         s3=s{i};
2116 %
2117 %         s = get(controls.popupmenu_precision,'String');
2118 %         i = get(controls.popupmenu_precision,'Value');
2119 %         s4=s{i};
2120 %
2121 %         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2122 %
2123 %         tp=construct_target(target_num);
2124 %
2125 %         instruction_matlab = ['robot=' s1 '(robot, tp, s3, s4, tool0, wobj0);'];
2126 %         fprintf('\n**********************************************\n');
2127 %         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2128 %         fprintf('**********************************************\n');
2129 %
2130 %         eval(instruction_matlab);
2131 %
2132 %      case 'MoveAbsJ'
2133 %
2134 %         s = get(controls.popupmenu_target_point,'String');
2135 %         target_num = get(controls.popupmenu_target_point,'Value');
2136 %         s2=s{target_num};
2137 %
2138 %         s = get(controls.popupmenu_speed,'String');
2139 %         i = get(controls.popupmenu_speed,'Value');
2140 %         s3=s{i};
2141 %
2142 %         s = get(controls.popupmenu_precision,'String');
2143 %         i = get(controls.popupmenu_precision,'Value');
2144 %         s4=s{i};
2145 %
2146 %         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2147 %
2148 %
2149 %         joint_coord=[targets{target_num}.q' 1e9, 1e9, 1e9, 1e9, 1e9, 1e9];
2150 %
2151 %         instruction_matlab = ['robot=' s1 '(robot, joint_coord, s3, s4, tool0, wobj0);'];
2152 %         fprintf('\n**********************************************\n');
2153 %         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2154 %         fprintf('**********************************************\n');
2155 %
2156 %         eval(instruction_matlab);
2157 %
2158 %
2159 %     case 'MoveC'
2160 %         s = get(controls.popupmenu_target_point,'String');
2161 %         target_num1 = get(controls.popupmenu_target_point,'Value');
2162 %         s2=s{target_num1};
2163 %
2164 %         s = get(controls.popupmenu_through_point,'String');
2165 %         target_num2 = get(controls.popupmenu_through_point,'Value');
2166 %         s3=s{target_num2};
2167 %
2168 %         s = get(controls.popupmenu_speed,'String');
2169 %         i = get(controls.popupmenu_speed,'Value');
2170 %         vel=s{i};
2171 %
2172 %         s = get(controls.popupmenu_precision,'String');
2173 %         i = get(controls.popupmenu_precision,'Value');
2174 %         prec=s{i};
2175 %
2176 %         instruction_rapid = [s1 ' ' s3 ',' s2 ',' vel ',' prec  ',tool0\Wobj:=wobj0;'];
2177 %
2178 %         tp2=construct_target(target_num1);
2179 %         tp1=construct_target(target_num2);%the through point
2180 %
2181 %         instruction_matlab = ['robot=' s1 '(robot, tp1, tp2, vel , prec);'];
2182 %         fprintf('\n**********************************************\n');
2183 %         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2184 %         fprintf('**********************************************\n');
2185 %
2186 %         eval(instruction_matlab);
2187 %
2188 %     otherwise disp('RAPID instruction not supported');
2189 % end
2190 
2191 % %reset position velocity acceleration and time
2192 % robot.q_vector=[];
2193 % robot.qd_vector=[];
2194 % robot.qdd_vector=[];
2195 % robot.time=[];
2196 %
2197 % update_T_Q();
2198 % update_sliders();
2199 % draw_target_points();
2200 
2201 %clf(controls.global_gui_handle)
2202 %drawnow()
2203 
2204 
2205 % --- Executes on button press in pushbutton_save_instruction.
2206 % function pushbutton_save_instruction_Callback(hObject, eventdata, handles)
2207 % % hObject    handle to pushbutton_save_instruction (see GCBO)
2208 % % eventdata  reserved - to be defined in a future version of MATLAB
2209 % % handles    structure with handles and user data (see GUIDATA)
2210 %
2211 % global controls targets robot program
2212 
2213 % %Get instruction name
2214 % instruction = [];
2215 %
2216 % s = get(controls.popupmenu_instruction,'String');
2217 % i = get(controls.popupmenu_instruction,'Value');
2218 % s1=s{i};
2219 %
2220 % s = get(controls.popupmenu_target_point,'String');
2221 % target_num = get(controls.popupmenu_target_point,'Value');
2222 % s2=s{target_num};
2223 %
2224 % s = get(controls.popupmenu_speed,'String');
2225 % i = get(controls.popupmenu_speed,'Value');
2226 % s3=s{i};
2227 %
2228 % s = get(controls.popupmenu_precision,'String');
2229 % i = get(controls.popupmenu_precision,'Value');
2230 % s4=s{i};
2231 
2232 
2233 % [instruction_matlab, instruction_rapid]=build_instruction_to_save();
2234 %
2235 %
2236 % %program.program_counter = program.program_counter + 1;
2237 % program.n_lines = program.n_lines + 1;
2238 %
2239 % program.rapid_lines{program.n_lines} = instruction_rapid;%[s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2240 % program.matlab_lines{program.n_lines} = instruction_matlab;%['robot=' s1 '(robot,' s2 ',' sprintf('''%s''', s3) ',' sprintf('''%s''', s4) ');'];
2241 %
2242 %
2243 % fprintf('\n**********************************************\n');
2244 % fprintf('Adding instruction to program.\nCurrent program:');
2245 % fprintf('\n**********************************************\n');
2246 % for i=1:program.n_lines,
2247 %     fprintf('\n%s', program.rapid_lines{i});
2248 % end
2249 % fprintf('\n**********************************************\n');
2250 % update_T_Q();
2251 % update_sliders();
2252 % draw_target_points()
2253 
2254 
2255 
2256 
2257 
2258 % --- Executes on key press with focus on slider_q1 and none of its controls.
2259 function slider_q1_KeyPressFcn(hObject, eventdata, handles)
2260 % hObject    handle to slider_q1 (see GCBO)
2261 % eventdata  structure with the following fields (see UICONTROL)
2262 %    Key: name of the key that was pressed, in lower case
2263 %    Character: character interpretation of the key(s) that was pressed
2264 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
2265 % handles    structure with handles and user data (see GUIDATA)
2266 
2267 
2268 
2269 
2270 % --- Executes on selection change in popupmenu_base_end.
2271 function popupmenu_base_end_Callback(hObject, eventdata, handles)
2272 % hObject    handle to popupmenu_base_end (see GCBO)
2273 % eventdata  reserved - to be defined in a future version of MATLAB
2274 % handles    structure with handles and user data (see GUIDATA)
2275 
2276 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_base_end contents as cell array
2277 %        contents{get(hObject,'Value')} returns selected item from popupmenu_base_end
2278 
2279 
2280 % --- Executes during object creation, after setting all properties.
2281 function popupmenu_base_end_CreateFcn(hObject, eventdata, handles)
2282 % hObject    handle to popupmenu_base_end (see GCBO)
2283 % eventdata  reserved - to be defined in a future version of MATLAB
2284 % handles    empty - handles not created until after all CreateFcns called
2285 
2286 global controls
2287 controls.popupmenu_base_end=hObject;
2288 
2289 % Hint: popupmenu controls usually have a white background on Windows.
2290 %       See ISPC and COMPUTER.
2291 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
2292     set(hObject,'BackgroundColor','white');
2293 end
2294 
2295 
2296 
2297 
2298 %%%%%%%%%%%%%%%%%%%%%%%%%%
2299 %   REST OF FUNCTIONS TO HELP DIALOGS
2300 %%%%%%%%%%%%%%%%%%%%%%%%%%%%
2301 
2302 %update the values from T to the dialog
2303 function update_T_Q()
2304 
2305 global robot controls
2306 
2307 
2308 T = directkinematic(robot, robot.q);
2309 
2310 %if there is a tool attached to it, consider it in the computation of
2311 % direct kinematics
2312 if isfield(robot, 'tool')
2313     T=T*robot.tool.TCP; 
2314 end
2315 
2316 %Update T
2317 set(controls.edit_nx, 'String', num2str(T(1,1), '%.3f'));
2318 set(controls.edit_ny, 'String', num2str(T(2,1), '%.3f'));
2319 set(controls.edit_nz, 'String', num2str(T(3,1), '%.3f'));
2320 set(controls.edit_ox, 'String', num2str(T(1,2), '%.3f'));
2321 set(controls.edit_oy, 'String', num2str(T(2,2), '%.3f'));
2322 set(controls.edit_oz, 'String', num2str(T(3,2), '%.3f'));
2323 set(controls.edit_ax, 'String', num2str(T(1,3), '%.3f'));
2324 set(controls.edit_ay, 'String', num2str(T(2,3), '%.3f'));
2325 set(controls.edit_az, 'String', num2str(T(3,3), '%.3f'));
2326 set(controls.edit_pxt, 'String', num2str(T(1,4), '%.3f'));
2327 set(controls.edit_pyt, 'String', num2str(T(2,4), '%.3f'));
2328 set(controls.edit_pzt, 'String', num2str(T(3,4), '%.3f'));
2329 
2330 %convert T to quaternion
2331 Q = T2quaternion(T);
2332 Q=real(Q);
2333 set(controls.edit_Q0, 'String', num2str(Q(1),'%.3f'));
2334 set(controls.edit_Q1, 'String', num2str(Q(2), '%.3f'));
2335 set(controls.edit_Q2, 'String', num2str(Q(3), '%.3f'));
2336 set(controls.edit_Q3, 'String', num2str(Q(4), '%.3f'));
2337 %the same position as in T
2338 set(controls.edit_pxq, 'String', num2str(T(1,4), '%.3f'));
2339 set(controls.edit_pyq, 'String', num2str(T(2,4), '%.3f'));
2340 set(controls.edit_pzq, 'String', num2str(T(3,4), '%.3f'));
2341 
2342 
2343 %obtain the values of T Q and posittion from
2344 function [T,Q,P]=get_TQ_from_dialog()
2345 
2346 global robot controls
2347 
2348 
2349 T = eye(4);
2350 
2351 %Update T
2352 T(1,1)=sscanf(get(controls.edit_nx, 'String'),'%f');
2353 T(2,1)=sscanf(get(controls.edit_ny, 'String'), '%f');
2354 T(3,1)=sscanf(get(controls.edit_nz, 'String'), '%f');
2355 T(1,2)=sscanf(get(controls.edit_ox, 'String'), '%f');
2356 T(2,2)=sscanf(get(controls.edit_oy, 'String'), '%f');
2357 T(3,2)=sscanf(get(controls.edit_oz, 'String'), '%f');
2358 T(1,3)=sscanf(get(controls.edit_ax, 'String'), '%f');
2359 T(2,3)=sscanf(get(controls.edit_ay, 'String'), '%f');
2360 T(3,3)=sscanf(get(controls.edit_az, 'String'), '%f');
2361 T(1,4)=sscanf(get(controls.edit_pxt, 'String'), '%f');
2362 T(2,4)=sscanf(get(controls.edit_pyt, 'String'), '%f');
2363 T(3,4)=sscanf(get(controls.edit_pzt, 'String'), '%f');
2364  
2365 Q = zeros(1,4);
2366 Q(1)=sscanf(get(controls.edit_Q0, 'String'),'%f');
2367 Q(2)=sscanf(get(controls.edit_Q1, 'String'),'%f');
2368 Q(3)=sscanf(get(controls.edit_Q2, 'String'),'%f');
2369 Q(4)=sscanf(get(controls.edit_Q3, 'String'),'%f');
2370 
2371 P(1)=sscanf(get(controls.edit_pxq, 'String'), '%f');
2372 P(2)=sscanf(get(controls.edit_pyq, 'String'), '%f');
2373 P(3)=sscanf(get(controls.edit_pzq, 'String'), '%f');
2374 
2375 
2376 %update the position of the sliders as well as the edit boxes associated to
2377 %them
2378 function update_sliders()
2379 
2380 global robot controls
2381 
2382 error = test_joints(robot, robot.q);
2383 
2384 %do not update in case of any joint exceeds its range
2385 if error == 1
2386     return;
2387 end
2388 
2389 if robot.DOF > 0
2390     %Update the sliders
2391     set(controls.slider_q1, 'Value', robot.q(1)*180/pi);
2392     %update the edit controls associated to them
2393     set(controls.edit_q1, 'String', num2str(robot.q(1)*180/pi,3));
2394 end
2395 if robot.DOF > 1
2396     set(controls.slider_q2, 'Value', robot.q(2)*180/pi);
2397     set(controls.edit_q2, 'String', num2str(robot.q(2)*180/pi,3));
2398 end
2399 if robot.DOF > 2
2400     set(controls.slider_q3, 'Value', robot.q(3)*180/pi);
2401     set(controls.edit_q3, 'String', num2str(robot.q(3)*180/pi,3));
2402 end
2403 if robot.DOF > 3
2404     set(controls.slider_q4, 'Value', robot.q(4)*180/pi);
2405     set(controls.edit_q4, 'String', num2str(robot.q(4)*180/pi,3));
2406 end
2407 if robot.DOF > 4
2408     set(controls.slider_q5, 'Value', robot.q(5)*180/pi);
2409     set(controls.edit_q5, 'String', num2str(robot.q(5)*180/pi,3));
2410 end
2411 if robot.DOF > 5
2412     set(controls.slider_q6, 'Value', robot.q(6)*180/pi);
2413     set(controls.edit_q6, 'String', num2str(robot.q(6)*180/pi,3));
2414 end
2415 
2416 if robot.DOF > 6
2417     disp('Not showing q when robot.DOF >= 7');
2418 end
2419 
2420 
2421 %plan a lineal path interpolating linearly the joint coordinates
2422 function path=lineal_path_plan(q_ini, q_final)
2423 
2424 global robot
2425 
2426 %make num_points
2427 num_points = 15;
2428 
2429 path = [];
2430 
2431 for i=1:robot.DOF,
2432     delta = (q_final(i)-q_ini(i))/num_points;
2433     for j=1:num_points-1;
2434         path(i,j)= q_ini(i)+delta*j;
2435     end
2436     path(i,num_points)=q_final(i);
2437 end
2438 
2439 
2440 
2441 %plan a lineal path interpolating linearly the joint coordinates
2442 function path=lineal_path_planXYZ(P_ini, P_final)
2443 
2444 global robot
2445 
2446 %make num_points
2447 num_points = 5;
2448 
2449 path = [];
2450 
2451 for i=1:3, %3, X, Y, Z
2452     delta = (P_final(i)-P_ini(i))/num_points;
2453     for j=1:num_points-1;
2454         path(i,j)= P_ini(i)+delta*j;
2455     end
2456     path(i,num_points)=P_final(i);
2457 end
2458 
2459 
2460 
2461 %follow a line in global coordinates
2462 function follow_line(P_ini,P_final)
2463 global robot configuration
2464 %current end effector's position/orientation
2465 T = directkinematic(robot, robot.q);
2466 
2467 current_conf = compute_configuration(robot, robot.q);   
2468 
2469 path = lineal_path_planXYZ(P_ini,P_final);
2470 
2471 for i=1:size(path,2),
2472     T(1:3,4)=path(:,i);
2473     %several solutions are provided
2474     qinv = inversekinematic(robot, T); 
2475     %choose the closest to current position
2476    
2477     q=select_closest_joint_coordinates(qinv, robot.q);
2478     
2479     robot.q = q;
2480     
2481     %Test whether there are joints outside mechanical limits
2482     error = test_joints(robot, robot.q);
2483     
2484     drawrobot3d(robot, robot.q);
2485     
2486     %current_conf = compute_configuration(robot, robot.q);
2487     
2488     if error == 1
2489         disp('\nAn error has occurred during the trajectory. Please check angle ranges');
2490         break;
2491     end
2492 end
2493 update_T_Q();
2494 update_sliders();
2495 plot3(path(1,:),path(2,:),path(3,:),'k', 'LineWidth', 3);
2496 
2497 draw_target_points();
2498 
2499 
2500 
2501 %draw the targets points on the robots figure
2502 function draw_target_points()
2503 global targets configuration controls
2504 figure(configuration.figure.robot)
2505 
2506 s=0.1; %scale
2507 
2508 for i=1:controls.num_target_points,
2509     
2510     T = targets{i}.T;
2511     p0=T(1:3,4)';
2512     x=p0+s*T(1:3,1)';
2513     y=p0+s*T(1:3,2)';
2514     z=p0+s*T(1:3,3)';
2515     
2516     vect_arrow(p0,x,'r')
2517     vect_arrow(p0,y,'g')
2518     vect_arrow(p0,z,'b')
2519     %plot vector names X_i Y_i Z_i
2520     text(x(1)+0.01, x(2)+0.01, x(3)+0.01,targets{i}.name, 'FontWeight', 'bold', 'HorizontalAlignment', 'Center', 'FontSize', 14);
2521 end
2522 
2523 
2524 function tp = construct_target(target_num)
2525 
2526 global targets
2527 
2528 T = targets{target_num}.T;
2529 Q = targets{target_num}.Q;
2530 conf = targets{target_num}.conf;
2531 
2532 tp = [[T(1,4),T(2,4),T(3,4)],[Q(1), Q(2), Q(3), Q(4)], [conf(1), conf(2), conf(3), conf(4)], [9E9,9E9,9E9,9E9,9E9,9E9]];
2533 
2534 
2535 
2536 %decode a line into the different variables of a target point
2537 function [name, Q, T, conf]=get_targetpoint_from_string(a)
2538 
2539 %start decoding the line
2540 [name, remain] = strtok(a,'=');
2541 [token, remain] = strtok(remain,'[');
2542 
2543 eval(['target=' remain]);
2544 
2545 px=target(1);
2546 py=target(2);
2547 pz=target(3);
2548 
2549 Q = target(4:7);
2550 T = quaternion2T(Q, [px py pz]);
2551 
2552 conf = target(8:11);
2553 
2554 
2555 
2556 
2557 function [instruction_matlab, instruction_rapid, tp1, tp2, joint_coord]=build_instruction_to_eval() 
2558 
2559 global controls targets robot
2560 
2561 s = get(controls.popupmenu_instruction,'String');
2562 i = get(controls.popupmenu_instruction,'Value');
2563 s1=s{i};
2564 
2565 tp1=[];
2566 tp1_name=[];
2567 tp2=[];
2568 tp2_name = [];
2569 joint_coord=[];
2570 switch s1
2571     case {'MoveJ','MoveL'}
2572         
2573         s = get(controls.popupmenu_target_point,'String');
2574         target_num = get(controls.popupmenu_target_point,'Value');
2575         s2=s{target_num};
2576         
2577         s = get(controls.popupmenu_speed,'String');
2578         i = get(controls.popupmenu_speed,'Value');
2579         s3=s{i};
2580         
2581         s = get(controls.popupmenu_precision,'String');
2582         i = get(controls.popupmenu_precision,'Value');
2583         s4=s{i};
2584         
2585         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2586         
2587         tp1=construct_target(target_num);
2588         
2589         
2590         s = get(controls.popupmenu_target_point,'String');
2591         target_num = get(controls.popupmenu_target_point,'Value');
2592         tp1_name=s{target_num};
2593 
2594         instruction_matlab = ['robot=' s1 '(robot, tp1,' sprintf(' ''%s'' ', s3) ',' sprintf(' ''%s'' ', s4) ', robot.tool0, robot.wobj0);'];
2595         fprintf('\n**********************************************\n');
2596         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2597         fprintf('**********************************************\n');
2598         
2599        % eval(instruction_matlab);
2600         
2601      case 'MoveAbsJ'
2602         
2603         s = get(controls.popupmenu_target_point,'String');
2604         target_num = get(controls.popupmenu_target_point,'Value');
2605         s2=s{target_num};
2606         
2607         s = get(controls.popupmenu_speed,'String');
2608         i = get(controls.popupmenu_speed,'Value');
2609         s3=s{i};
2610         
2611         s = get(controls.popupmenu_precision,'String');
2612         i = get(controls.popupmenu_precision,'Value');
2613         s4=s{i};
2614         
2615         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2616         
2617         
2618         joint_coord=[targets{target_num}.q' 1e9, 1e9, 1e9, 1e9, 1e9, 1e9];
2619         
2620         
2621         instruction_matlab = ['robot=' s1 '(robot, joint_coord,' sprintf(' ''%s'' ', s3) ',' sprintf(' ''%s'' ', s4) ', robot.tool0, robot.wobj0);'];
2622         fprintf('\n**********************************************\n');
2623         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2624         fprintf('**********************************************\n');
2625         
2626        % eval(instruction_matlab);
2627         
2628         
2629     case 'MoveC'
2630         s = get(controls.popupmenu_target_point,'String');
2631         target_num1 = get(controls.popupmenu_target_point,'Value');
2632         s2=s{target_num1};
2633         
2634         s = get(controls.popupmenu_through_point,'String');
2635         target_num2 = get(controls.popupmenu_through_point,'Value');
2636         s3=s{target_num2};
2637         
2638         s = get(controls.popupmenu_speed,'String');
2639         i = get(controls.popupmenu_speed,'Value');
2640         vel=s{i};
2641         
2642         s = get(controls.popupmenu_precision,'String');
2643         i = get(controls.popupmenu_precision,'Value');
2644         prec=s{i};
2645         
2646         instruction_rapid = [s1 ' ' s3 ',' s2 ',' vel ',' prec  ',tool0\Wobj:=wobj0;'];
2647         
2648         tp2=construct_target(target_num1);
2649         tp1=construct_target(target_num2);%the through point
2650         
2651         instruction_matlab = ['robot=' s1 '(robot, tp1, tp2, ' sprintf(' ''%s'' ', vel) ',' sprintf(' ''%s'' ', prec) ', robot.tool0, robot.wobj0);'];
2652         fprintf('\n**********************************************\n');
2653         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2654         fprintf('**********************************************\n');
2655         
2656        % eval(instruction_matlab);
2657         
2658     otherwise disp('RAPID instruction not supported');
2659 end
2660 
2661 
2662 
2663 function [instruction_matlab, instruction_rapid]=build_instruction_to_save() 
2664 
2665 global controls targets robot
2666 
2667 s = get(controls.popupmenu_instruction,'String');
2668 i = get(controls.popupmenu_instruction,'Value');
2669 s1=s{i};
2670 
2671 tp1=[];
2672 tp1_name=[];
2673 tp2=[];
2674 tp2_name = [];
2675 joint_coord=[];
2676 switch s1
2677     case {'MoveJ','MoveL'}
2678         
2679         s = get(controls.popupmenu_target_point,'String');
2680         target_num = get(controls.popupmenu_target_point,'Value');
2681         s2=s{target_num};
2682         
2683         s = get(controls.popupmenu_speed,'String');
2684         i = get(controls.popupmenu_speed,'Value');
2685         s3=s{i};
2686         
2687         s = get(controls.popupmenu_precision,'String');
2688         i = get(controls.popupmenu_precision,'Value');
2689         s4=s{i};
2690         
2691         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2692         
2693               
2694         
2695         s = get(controls.popupmenu_target_point,'String');
2696         target_num = get(controls.popupmenu_target_point,'Value');
2697         tp_name=s{target_num};
2698 
2699         instruction_matlab = ['robot=' s1 '(robot,' tp_name ',' ...
2700             sprintf(' ''%s'' ', s3) ',' sprintf(' ''%s'' ', s4) ', robot.tool0, robot.wobj0);'];
2701         fprintf('\n**********************************************\n');
2702         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2703         fprintf('**********************************************\n');
2704         
2705        % eval(instruction_matlab);
2706         
2707      case 'MoveAbsJ'
2708         
2709         s = get(controls.popupmenu_target_point,'String');
2710         target_num = get(controls.popupmenu_target_point,'Value');
2711         s2=s{target_num};
2712         
2713         s = get(controls.popupmenu_speed,'String');
2714         i = get(controls.popupmenu_speed,'Value');
2715         s3=s{i};
2716         
2717         s = get(controls.popupmenu_precision,'String');
2718         i = get(controls.popupmenu_precision,'Value');
2719         s4=s{i};
2720         
2721         instruction_rapid = [s1 ' ' s2 ',' s3 ',' s4 ',tool0\Wobj:=wobj0;'];
2722         
2723         
2724         %joint_coord=[targets{target_num}.q' 1e9, 1e9, 1e9, 1e9, 1e9, 1e9];
2725         
2726         s = get(controls.popupmenu_target_point,'String');
2727         target_num = get(controls.popupmenu_target_point,'Value');
2728         tp_name=s{target_num};
2729         
2730         instruction_matlab = ['robot=' s1 '(robot,' tp_name ','...
2731             sprintf(' ''%s'' ', s3) ',' sprintf(' ''%s'' ', s4) ', robot.tool0, robot.wobj0);'];
2732         fprintf('\n**********************************************\n');
2733         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2734         fprintf('**********************************************\n');
2735         
2736        % eval(instruction_matlab);
2737         
2738         
2739     case 'MoveC'
2740         s = get(controls.popupmenu_target_point,'String');
2741         target_num1 = get(controls.popupmenu_target_point,'Value');
2742         s2=s{target_num1};
2743         
2744         s = get(controls.popupmenu_through_point,'String');
2745         target_num2 = get(controls.popupmenu_through_point,'Value');
2746         s3=s{target_num2};
2747         
2748         s = get(controls.popupmenu_speed,'String');
2749         i = get(controls.popupmenu_speed,'Value');
2750         vel=s{i};
2751         
2752         s = get(controls.popupmenu_precision,'String');
2753         i = get(controls.popupmenu_precision,'Value');
2754         prec=s{i};
2755         
2756         instruction_rapid = [s1 ' ' s3 ',' s2 ',' vel ',' prec  ',tool0\Wobj:=wobj0;'];
2757         
2758         
2759         
2760         instruction_matlab = ['robot=' s1 '(robot,' s3 ',' s2 ', ' sprintf(' ''%s'' ', vel) ',' sprintf(' ''%s'' ', prec) ', robot.tool0, robot.wobj0);'];
2761         fprintf('\n**********************************************\n');
2762         fprintf('Evaluating instruction:\nRAPID:\n\t%s\nMatlab:\n\t%s\n', instruction_rapid, instruction_matlab);
2763         fprintf('**********************************************\n');
2764         
2765        % eval(instruction_matlab);
2766         
2767     otherwise disp('RAPID instruction not supported');
2768 end
2769 
2770 
2771 % --- Executes on selection change in popupmenu_line_reorient.
2772 function popupmenu_line_reorient_Callback(hObject, eventdata, handles)
2773 % hObject    handle to popupmenu_line_reorient (see GCBO)
2774 % eventdata  reserved - to be defined in a future version of MATLAB
2775 % handles    structure with handles and user data (see GUIDATA)
2776 
2777 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_line_reorient contents as cell array
2778 %        contents{get(hObject,'Value')} returns selected item from popupmenu_line_reorient
2779 
2780 
2781 % --- Executes during object creation, after setting all properties.
2782 function popupmenu_line_reorient_CreateFcn(hObject, eventdata, handles)
2783 % hObject    handle to popupmenu_line_reorient (see GCBO)
2784 % eventdata  reserved - to be defined in a future version of MATLAB
2785 % handles    empty - handles not created until after all CreateFcns called
2786 
2787 % Hint: popupmenu controls usually have a white background on Windows.
2788 %       See ISPC and COMPUTER.
2789 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
2790     set(hObject,'BackgroundColor','white');
2791 end
2792 
2793 
2794 global controls
2795 controls.popupmenu_line_reorient=hObject;
2796 
2797 
2798 
2799 % SHOWS CURRENT TARGET POINT IN RAPID on the command line
2800 % A rounding towards the 4 decimal point is performed, in order to make the
2801 % target points more readable. Rounding errors may cause differences in the
2802 % computed configurations from the original point. Please be aware that the
2803 % configuration corresponds to the point and orientation written in the
2804 % point.
2805 %
2806 % --- Executes on button press in pushbutton_show_target_point.
2807 function pushbutton_show_target_point_Callback(hObject, eventdata, handles)
2808 % hObject    handle to pushbutton_show_target_point (see GCBO)
2809 % eventdata  reserved - to be defined in a future version of MATLAB
2810 % handles    structure with handles and user data (see GUIDATA)
2811 
2812 global controls targets robot
2813 
2814 T = directkinematic(robot, robot.q);
2815 
2816 %if there is a tool attached to it, consider it in the computation of
2817 % direct kinematics
2818 if isfield(robot, 'tool')
2819     T=T*robot.tool.TCP; 
2820     disp('Show Target Point: The tool has been considered in the Target Point! OK!')
2821 end
2822 
2823 %convert to quaternion
2824 Q = T2quaternion(T);
2825 %avoid imaginari parts
2826 Q = real(Q);
2827 conf = compute_configuration(robot, robot.q);
2828 
2829 %the target points written to the command line are rounded to a 4 digit
2830 %precision. This may cause discrepancies in the cf1, cf4 or cf6 variables
2831 %(the configuration in RAPID). Thus, the points are first rounded, next,
2832 %the inversekinematic is computed for the rounded points the closest
2833 %solution
2834 tp=round_4_digits(T, Q, conf);
2835 
2836 fprintf('\n\ntp:=[[%.4f,%.4f,%.4f],[%.4f,%.4f,%.4f,%.4f],[%d,%d,%d,%d],[9E9,9E9,9E9,9E9,9E9,9E9]];\n\n',tp(1),tp(2),tp(3),tp(4),tp(5),tp(6),tp(7),tp(8),tp(9),tp(10),tp(11));
2837 
2838 
2839 function tp=round_4_digits(T, Q, conf)
2840 
2841 global robot
2842 %tp is rounded to the 4 digits
2843 tp = [round(10000*[T(1,4),T(2,4),T(3,4)])/10000,round(10000*[Q(1), Q(2), Q(3), Q(4)])/10000, [conf(1), conf(2), conf(3), conf(4)], [9E9,9E9,9E9,9E9,9E9,9E9]];
2844 
2845 %Compute the transformation for the rounded target point
2846 T_target = transform_to_homogeneous(tp);
2847 
2848 %if there is a tool attached to it, consider it in the computation of
2849 % inverse kinematics
2850 if isfield(robot, 'tool')
2851     T = T_target/robot.tool.TCP;
2852 end
2853 
2854 q=inversekinematic(robot, T_target);
2855 
2856 % s=[];
2857 % %now, find the closest q to the current joint values robot.q
2858 % for i=1:robot.DOF,
2859 %     s=[s sum(abs(q(:,i)-robot.q))];
2860 % end
2861 % [val,i]=min(s);
2862 
2863 index=find_closest(q, robot.q);
2864 
2865 %now, q corresponds to the joint values that correspond to the rounded
2866 %target point
2867 T = directkinematic(robot, q(:,index));
2868 
2869 %convert to quaternion
2870 Q = T2quaternion(T);
2871 %avoid imaginari parts
2872 Q = real(Q);
2873 conf = compute_configuration(robot, q(:,index));
2874 
2875 tp = [[T(1,4),T(2,4),T(3,4)],[Q(1), Q(2), Q(3), Q(4)], [conf(1), conf(2), conf(3), conf(4)], [9E9,9E9,9E9,9E9,9E9,9E9]];
2876 
2877 
2878 
2879 % --- Executes on key press with focus on pushbutton_show_target_point and none of its controls.
2880 % SHOWS THE CURRENT TP WHERE THE ROBOT IS SITUATED
2881 function pushbutton_show_target_point_KeyPressFcn(hObject, eventdata, handles)
2882 % hObject    handle to pushbutton_show_target_point (see GCBO)
2883 % eventdata  structure with the following fields (see UICONTROL)
2884 %    Key: name of the key that was pressed, in lower case
2885 %    Character: character interpretation of the key(s) that was pressed
2886 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
2887 % handles    structure with handles and user data (see GUIDATA)
2888 
2889 
2890 
2891 
2892 
2893 % --- Executes on selection change in popupmenu_resolution.
2894 function popupmenu_resolution_Callback(hObject, eventdata, handles)
2895 % hObject    handle to popupmenu_resolution (see GCBO)
2896 % eventdata  reserved - to be defined in a future version of MATLAB
2897 % handles    structure with handles and user data (see GUIDATA)
2898 
2899 % Hints: contents = cellstr(get(hObject,'String')) returns popupmenu_resolution contents as cell array
2900 %        contents{get(hObject,'Value')} returns selected item from popupmenu_resolution
2901 
2902 
2903 % --- Executes during object creation, after setting all properties.
2904 function popupmenu_resolution_CreateFcn(hObject, eventdata, handles)
2905 % hObject    handle to popupmenu_resolution (see GCBO)
2906 % eventdata  reserved - to be defined in a future version of MATLAB
2907 % handles    empty - handles not created until after all CreateFcns called
2908 
2909 global controls
2910 controls.popupmenu_resolution=hObject;
2911 
2912 
2913 % Hint: popupmenu controls usually have a white background on Windows.
2914 %       See ISPC and COMPUTER.
2915 if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
2916     set(hObject,'BackgroundColor','white');
2917 end
2918 
2919 
2920 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2921 % RESETS THE ROBOTS CURRENT JOINTS TO 0
2922 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2923 % --- Executes on button press in pushbutton_reset_all.
2924 function pushbutton_reset_all_Callback(hObject, eventdata, handles)
2925 % hObject    handle to pushbutton_reset_all (see GCBO)
2926 % eventdata  reserved - to be defined in a future version of MATLAB
2927 % handles    structure with handles and user data (see GUIDATA)
2928 
2929 global configuration robot controls
2930 
2931 update_T_Q();
2932 figure(configuration.figure.robot);
2933 
2934 
2935 drawrobot3d(robot, robot.q);
2936 
2937 final_joints = zeros(1,robot.DOF);
2938 %Use MoveAbsJ to move to zero coordinates
2939 command =[' MoveAbsJ(final_joints,' sprintf(' ''%s'' ', 'vmax') ',' sprintf(' ''%s'' ', 'fine') ',robot.tool0, robot.wobj0);'];
2940 eval(command);
2941 robot.q=final_joints;
2942 
2943 update_T_Q();
2944 draw_target_points();
2945 update_sliders();
2946 
2947 
2948 % --- Executes on key press with focus on pushbutton_save_target_point and none of its controls.
2949 function pushbutton_save_target_point_KeyPressFcn(hObject, eventdata, handles)
2950 % hObject    handle to pushbutton_save_target_point (see GCBO)
2951 % eventdata  structure with the following fields (see UICONTROL)
2952 %    Key: name of the key that was pressed, in lower case
2953 %    Character: character interpretation of the key(s) that was pressed
2954 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
2955 % handles    structure with handles and user data (see GUIDATA)
2956 
2957 
2958 
2959 
2960 
2961 
2962 
2963 
2964 function index=find_closest(q,qi)
2965 
2966 qq=zeros(size(q,1),size(q,2));
2967 for i=1:size(q,2),
2968     qq(:,i)=qi;
2969 end
2970 
2971 a=sum((q-qq).^2);
2972 
2973 [val, index] = min(a);
2974 
2975 
2976 % --- Executes on button press in pushbutton_load_robot.
2977 function pushbutton_load_robot_Callback(hObject, eventdata, handles)
2978 % hObject    handle to pushbutton_load_robot (see GCBO)
2979 % eventdata  reserved - to be defined in a future version of MATLAB
2980 % handles    structure with handles and user data (see GUIDATA)
2981 global configuration robot controls
2982 
2983 %
2984 robot=load_robot()
2985 
2986 % --- Executes on key press with focus on pushbutton_load_robot and none of its controls.
2987 function pushbutton_load_robot_KeyPressFcn(hObject, eventdata, handles)
2988 % hObject    handle to pushbutton_load_robot (see GCBO)
2989 % eventdata  structure with the following fields (see UICONTROL)
2990 %    Key: name of the key that was pressed, in lower case
2991 %    Character: character interpretation of the key(s) that was pressed
2992 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
2993 % handles    structure with handles and user data (see GUIDATA)
2994 
2995 
2996 
2997 % --- Executes on button press in push_button_load_end_tool.
2998 function push_button_load_end_tool_Callback(hObject, eventdata, handles)
2999 % hObject    handle to push_button_load_end_tool (see GCBO)
3000 % eventdata  reserved - to be defined in a future version of MATLAB
3001 % handles    structure with handles and user data (see GUIDATA)
3002 global configuration robot controls
3003 %
3004 
3005 robot.tool=load_robot
3006 
3007 % --- Executes on key press with focus on push_button_load_end_tool and none of its controls.
3008 function push_button_load_end_tool_KeyPressFcn(hObject, eventdata, handles)
3009 % hObject    handle to push_button_load_end_tool (see GCBO)
3010 % eventdata  structure with the following fields (see UICONTROL)
3011 %    Key: name of the key that was pressed, in lower case
3012 %    Character: character interpretation of the key(s) that was pressed
3013 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
3014 % handles    structure with handles and user data (see GUIDATA)
3015 
3016 
3017 
3018 
3019 % --- Executes on button press in push_button_load_environment.
3020 function push_button_load_environment_Callback(hObject, eventdata, handles)
3021 % hObject    handle to push_button_load_environment (see GCBO)
3022 % eventdata  reserved - to be defined in a future version of MATLAB
3023 % handles    structure with handles and user data (see GUIDATA)
3024 
3025 global configuration robot controls
3026 %
3027 robot.equipment=load_robot
3028 
3029 % --- Executes on key press with focus on push_button_load_environment and none of its controls.
3030 function push_button_load_environment_KeyPressFcn(hObject, eventdata, handles)
3031 % hObject    handle to push_button_load_environment (see GCBO)
3032 % eventdata  structure with the following fields (see UICONTROL)
3033 %    Key: name of the key that was pressed, in lower case
3034 %    Character: character interpretation of the key(s) that was pressed
3035 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
3036 % handles    structure with handles and user data (see GUIDATA)
3037 
3038 
3039 % --- Executes on key press with focus on pushbutton_simulate_instruction and none of its controls.
3040 function pushbutton_simulate_instruction_KeyPressFcn(hObject, eventdata, handles)
3041 % hObject    handle to pushbutton_simulate_instruction (see GCBO)
3042 % eventdata  structure with the following fields (see UICONTROL)
3043 %    Key: name of the key that was pressed, in lower case
3044 %    Character: character interpretation of the key(s) that was pressed
3045 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
3046 % handles    structure with handles and user data (see GUIDATA)
3047 
3048 
3049 % --- Executes on button press in push_button_save_TP.
3050 function push_button_save_TP_Callback(hObject, eventdata, handles)
3051 % hObject    handle to push_button_save_TP (see GCBO)
3052 % eventdata  reserved - to be defined in a future version of MATLAB
3053 % handles    structure with handles and user data (see GUIDATA)
3054 
3055 
3056 global controls targets robot
3057 
3058 
3059 %always require a filename
3060 [filename, pathname] = uiputfile('*.m','Save as .m');
3061 if isequal(filename,0) || isequal(pathname,0)
3062    disp('User selected Cancel')
3063    return;
3064 else
3065    disp(['User selected',fullfile(pathname,filename)])
3066 end
3067 
3068 
3069 %open selected file
3070 fp=fopen(fullfile(pathname, filename),'w');
3071 
3072 fprintf('Saving %d target points to file', controls.num_target_points);
3073 
3074 for i=1:controls.num_target_points,
3075     %tp = construct_target(i);
3076   
3077     tp=round_4_digits(targets{i}.T, targets{i}.Q, targets{i}.conf);
3078     ejes_ext = '[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];';
3079     %a_rap = sprintf('%s:=[[%.5f, %.5f, %.5f],[%.5f, %.5f, %.5f, %.5f], [%.5f, %.5f, %.5f, %.5f], %s',...
3080 %     targets{i}.name, targets{i}.T(1,4),targets{i}.T(2,4),targets{i}.T(3,4),...
3081 %             targets{i}.Q(1), targets{i}.Q(2), targets{i}.Q(3), targets{i}.Q(4),...
3082 %         targets{i}.conf(1), targets{i}.conf(2), targets{i}.conf(3), targets{i}.conf(4), ejes_ext);
3083 %     a_mat = sprintf('%s=[[%.4f, %.4f, %.4f],[%.4f, %.4f, %.4f, %.4f], [%d, %d, %d, %d], %s',...
3084 %     targets{i}.name, targets{i}.T(1,4),targets{i}.T(2,4),targets{i}.T(3,4),...
3085 %             targets{i}.Q(1), targets{i}.Q(2), targets{i}.Q(3), targets{i}.Q(4),...
3086 %         targets{i}.conf(1), targets{i}.conf(2), targets{i}.conf(3), targets{i}.conf(4), ejes_ext);
3087     a_mat = sprintf('%s=[[%.4f, %.4f, %.4f],[%.4f, %.4f, %.4f, %.4f], [%d, %d, %d, %d], %s',...
3088     targets{i}.name, tp(1), tp(2), tp(3),... %name, position
3089             tp(4), tp(5), tp(6), tp(7),... %quaternion
3090         tp(8), tp(9),tp(10), tp(11), ejes_ext); %configuration
3091     %write file
3092     %fprintf(fp, '\n%%%s', a_rap);
3093     fprintf(fp, '\n%s', a_mat);
3094 end
3095 
3096 fclose(fp);
3097 
3098 
3099 % --- Executes on button press in pushbutton_refresh.
3100 function pushbutton_refresh_Callback(hObject, eventdata, handles)
3101 % hObject    handle to pushbutton_refresh (see GCBO)
3102 % eventdata  reserved - to be defined in a future version of MATLAB
3103 % handles    structure with handles and user data (see GUIDATA)
3104 global robot
3105 
3106 update_T_Q();
3107 update_sliders();
3108 draw_target_points();
3109 drawrobot3d(robot, robot.q)
3110 
3111 % --- Executes on key press with focus on pushbutton_refresh and none of its controls.
3112 function pushbutton_refresh_KeyPressFcn(hObject, eventdata, handles)
3113 % hObject    handle to pushbutton_refresh (see GCBO)
3114 % eventdata  structure with the following fields (see UICONTROL)
3115 %    Key: name of the key that was pressed, in lower case
3116 %    Character: character interpretation of the key(s) that was pressed
3117 %    Modifier: name(s) of the modifier key(s) (i.e., control, shift) pressed
3118 % handles    structure with handles and user data (see GUIDATA)
3119 
3120 
3121 
3122 
3123 % --- Executes on button press in pushbutton_load_piece.
3124 function pushbutton_load_piece_Callback(hObject, eventdata, handles)
3125 % hObject    handle to pushbutton_load_piece (see GCBO)
3126 % eventdata  reserved - to be defined in a future version of MATLAB
3127 % handles    structure with handles and user data (see GUIDATA)
3128 
3129 global robot
3130 
3131 robot.piece=load_robot();%('equipment/cylinders/cylinder_tiny');
3132 %robot.piece.T0(1:3,4)=[0 -0.45 0.2]';