%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DRAWROBOT3D_par(ROBOT, Q) 3D drawing with DH reference systems for a parallel robot. The parallel robot is divided into different serial arms, drawn altogether If the robot possesses 3D graphics, the function DRAW_LINK is called to represent the link in 3D. Otherwise a line connecting each consecutive reference system is plotted. See also DRAWROBOT3D, DENAVIT, DIRECTKINEMATIC, DRAW_LINK. Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. email: arturo.gil@umh.es date: 05/02/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % DRAWROBOT3D_par(ROBOT, Q) 0003 % 3D drawing with DH reference systems for a parallel robot. 0004 % The parallel robot is divided into different serial arms, drawn altogether 0005 % 0006 % If the robot possesses 3D graphics, the function DRAW_LINK is called to represent 0007 % the link in 3D. Otherwise a line connecting each consecutive reference 0008 % system is plotted. 0009 % 0010 % See also DRAWROBOT3D, DENAVIT, DIRECTKINEMATIC, DRAW_LINK. 0011 % 0012 % Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche. 0013 % email: arturo.gil@umh.es date: 05/02/2012 0014 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0015 0016 % Copyright (C) 2012, by Arturo Gil Aparicio 0017 % 0018 % This file is part of ARTE (A Robotics Toolbox for Education). 0019 % 0020 % ARTE is free software: you can redistribute it and/or modify 0021 % it under the terms of the GNU Lesser General Public License as published by 0022 % the Free Software Foundation, either version 3 of the License, or 0023 % (at your option) any later version. 0024 % 0025 % ARTE is distributed in the hope that it will be useful, 0026 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0027 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0028 % GNU Lesser General Public License for more details. 0029 % 0030 % You should have received a copy of the GNU Leser General Public License 0031 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0032 function drawrobot3d_par(robot, q, noclear) 0033 %global configuration 0034 0035 %origin=[0 0 0]; 0036 endpoints=[]; 0037 %draw every arm considered in the parallel mechanism 0038 k=1; 0039 for i=1:robot.nserial, 0040 r=eval(sprintf('robot.robot%d',i)); 0041 0042 fi=q(k:(k+r.DOF-1)); 0043 k=k+r.DOF; 0044 drawrobot3d(r,fi,0); 0045 0046 T=eval(sprintf('directkinematic(robot.robot%d, fi)',i)); 0047 destination=T(1:3,4); 0048 endpoints = [endpoints destination]; 0049 0050 end 0051 %Add the first point 0052 endpoints = [endpoints endpoints(:,1)]; 0053 0054 %now, draw a line between the end points including the first 0055 for i=1:robot.nserial, 0056 origin=endpoints(:,i); 0057 destination=endpoints(:,i+1); 0058 line([origin(1) destination(1)],[origin(2) destination(2)],[origin(3) destination(3)], 'Color', 'k' ,'LineWidth', 3 ); 0059 end 0060 0061 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0062 % draw equipment such as tables, conveyor velts... etc 0063 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0064 if isfield(robot, 'equipment') 0065 if robot.equipment.graphical.has_graphics 0066 draw_link(robot.equipment, 1, robot.equipment.T0); 0067 end 0068 if robot.equipment.graphical.draw_axes 0069 draw_axes(robot.equipment.T0, sprintf('X_{env%d}',0), sprintf('Y_{env%d}',0), sprintf('Z_{env%d}',0)); 0070 end 0071 end 0072 0073 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0074 % draw equipment tools 0075 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0076 if isfield(robot, 'tool') 0077 if robot.tool.graphical.has_graphics 0078 T=directkinematic(robot,q); 0079 draw_link(robot.tool, 1, T); 0080 end 0081 if robot.equipment.graphical.draw_axes 0082 draw_axes(T, sprintf('X_{tool%d}',0), sprintf('Y_{tool%d}',0), sprintf('Z_{tool%d}',0)); 0083 end 0084 end 0085 0086 0087 0088 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0089 % DRAW A PIECE GRABBED 0090 % this draws a piece grabbed at the robots end, or at the last known position 0091 % the variable robot.tool_activated=1 asumes that the piece has been grabbed by the end tool 0092 % robot.tool_activated==0 maintains the last known location for the pieced 0093 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0094 if isfield(robot, 'piece') 0095 if robot.piece.graphical.has_graphics 0096 if isfield(robot, 'tool') 0097 if robot.tool.piece_gripped==1 0098 %update the last known position and orientation of the robot 0099 %Trel is computed when the simulation_grip_piece function is 0100 %executed. Trel is the relative position and orientation of 0101 %the tool and the piece that assures that the piece is 0102 %picked at a constant and visually effective orientation. 0103 draw_link(robot.piece, 1, T07*(robot.tool.Trel)); 0104 else 0105 draw_link(robot.piece, 1, robot.piece.T0); 0106 end 0107 end 0108 else 0109 draw_link(robot.piece, 1, robot.piece.T0); 0110 end 0111 if robot.piece.graphical.draw_axes 0112 draw_axes(robot.piece.T0, sprintf('X_{piece%d}',0), sprintf('Y_{piece%d}',0), sprintf('Z_{piece%d}',0)); 0113 end 0114 end