%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DRAWROBOT3D(ROBOT, Q) 3D drawing with DH reference systems of the robot at the current joint coordinates Q. The robot parameters are stored in the variable ROBOT. Q = [q1 q2 ... qN] is a vector of joint values. 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 DENAVIT, DIRECTKINEMATIC, DRAW_LINK. Author: Arturo Gil. Universidad Miguel Hernandez de Elche. email: arturo.gil@umh.es date: 05/02/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % DRAWROBOT3D(ROBOT, Q) 3D drawing with DH reference systems of the robot at the 0003 % current joint coordinates Q. The robot parameters are stored in the 0004 % variable ROBOT. Q = [q1 q2 ... qN] is a vector of joint values. 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 DENAVIT, DIRECTKINEMATIC, DRAW_LINK. 0011 % 0012 % Author: Arturo Gil. Universidad Miguel Hernandez 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(robot, q, noclear) 0033 global configuration 0034 0035 0036 %if q is not specified, then assume zero pose 0037 if ~exist('q', 'var') 0038 q=zeros(1,robot.DOF) 0039 end 0040 0041 h=figure(configuration.figure.robot); 0042 0043 %get adjusted view 0044 [az,el] = view; 0045 0046 %retrieve the current zoom 0047 zz=get(gca,'CameraViewAngle'); 0048 0049 %delete figure 0050 if ~exist('noclear') 0051 clf(h), grid on, hold on 0052 end 0053 0054 xlabel('X (m)'), ylabel('Y (m)'), zlabel('Z (m)') 0055 0056 %apply the stored view 0057 view(az,el); 0058 %apply the zoom 0059 set(gca,'CameraViewAngle', zz); 0060 0061 origin = [0 0 0]; 0062 destination = []; 0063 color = ['r' 'g' 'b' 'c' 'm' 'y' 'k']; 0064 0065 %in case of a parallel robot, call drawrobot3d for each serial link 0066 if isfield(robot, 'parallel') 0067 drawrobot3d_par(robot, q); 0068 return; 0069 end 0070 0071 0072 V=[]; 0073 0074 T = robot.T0; 0075 for i=1:robot.DOF+1, 0076 0077 if robot.graphical.has_graphics 0078 draw_link(robot, i, T); 0079 else 0080 draw_link(robot, i, T); 0081 0082 destination=T(1:3,4); 0083 col = randperm(6); 0084 line([origin(1) destination(1)],[origin(2) destination(2)],[origin(3) destination(3)], 'Color', color(col(1)) ,'LineWidth', 3 ); 0085 origin = destination; 0086 end 0087 %draw D-H axes 0088 if robot.graphical.draw_axes 0089 draw_axes(T, sprintf('X_%d',i-1), sprintf('Y_%d',i-1), sprintf('Z_%d',i-1), robot.graphical.axes_scale); 0090 end 0091 %obtain transformation to draw the next link 0092 if i<= robot.DOF 0093 T=T*dh(robot, q, i); 0094 end 0095 end 0096 %save for later use 0097 T06robot=T; 0098 0099 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0100 % DRAW A TOOL AT THE ROBOT'S END 0101 % if there is a tool attached, draw it! 0102 % repeat the prior commands 0103 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0104 if isfield(robot, 'tool') 0105 0106 % Activate tool. Tools must be defined in a binary way. 0107 % This means that they are open or closed depending 0108 % on an joint 0109 if robot.tool.tool_open 0110 q_tool=ones(1,robot.tool.DOF); 0111 else 0112 q_tool=zeros(1,robot.tool.DOF); 0113 end 0114 robot.tool.T0=T; 0115 for i=1:robot.tool.DOF+1, 0116 if robot.tool.graphical.has_graphics 0117 draw_link(robot.tool, i, T); 0118 else 0119 draw_link(robot.tool, i, T); 0120 0121 destination=T(1:3,4); 0122 col = randperm(6); 0123 line([origin(1) destination(1)],[origin(2) destination(2)],[origin(3) destination(3)], 'Color', color(col(1)) ,'LineWidth', 3 ); 0124 origin = destination; 0125 end 0126 % if robot.tool.graphical.draw_axes 0127 % draw_axes(T, sprintf('X_{tool%d}',i-1), sprintf('Y_{tool%d}',i-1), sprintf('Z_{tool%d}',i-1)); 0128 % end 0129 %obtain transformation to draw the next link 0130 if (i)<= robot.tool.DOF 0131 T=T*dh(robot.tool, q_tool, i); 0132 end 0133 end 0134 %finally draw the Tool Center Point (TCP) 0135 if robot.tool.graphical.draw_axes 0136 draw_axes(T06robot*robot.tool.TCP, 'X_{toolTCP}', 'Y_{toolTCP}', 'Z_{toolTCP}'); 0137 end 0138 0139 %save for later use 0140 T07=T06robot*robot.tool.TCP; 0141 end 0142 0143 0144 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0145 % draw equipment such as tables, conveyor velts... etc 0146 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0147 if isfield(robot, 'equipment') 0148 if robot.equipment.graphical.has_graphics 0149 draw_link(robot.equipment, 1, robot.equipment.T0); 0150 end 0151 if robot.equipment.graphical.draw_axes 0152 draw_axes(robot.equipment.T0, sprintf('X_{env%d}',0), sprintf('Y_{env%d}',0), sprintf('Z_{env%d}',0)); 0153 end 0154 end 0155 0156 0157 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0158 % DRAW A PIECE GRABBED 0159 % this draws a piece grabbed at the robots end, or at the last known position 0160 % the variable robot.tool_activated=1 asumes that the piece has been grabbed by the end tool 0161 % robot.tool_activated==0 maintains the last known location for the pieced 0162 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0163 if isfield(robot, 'piece') 0164 if isfield(robot.piece, 'graphical') 0165 if robot.piece.graphical.has_graphics 0166 if isfield(robot, 'tool') 0167 if robot.tool.piece_gripped==1 0168 %update the last known position and orientation of the robot 0169 %Trel is computed when the simulation_grip_piece function is 0170 %executed. Trel is the relative position and orientation of 0171 %the tool and the piece that assures that the piece is 0172 %picked at a constant and visually effective orientation. 0173 draw_link(robot.piece, 1, T07*(robot.tool.Trel)); 0174 else 0175 draw_link(robot.piece, 1, robot.piece.T0); 0176 end 0177 end 0178 else 0179 draw_link(robot.piece, 1, robot.piece.T0); 0180 end 0181 if robot.piece.graphical.draw_axes 0182 draw_axes(robot.piece.T0, sprintf('X_{piece%d}',0), sprintf('Y_{piece%d}',0), sprintf('Z_{piece%d}',0)); 0183 end 0184 end 0185 end 0186 0187