%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% simulation_grab_piece Computes the variable robot.tool.Trel that defines the relative position and orientation of the piece with respect to the tool at the moment when the piece is grabbed. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % simulation_grab_piece 0003 % Computes the variable robot.tool.Trel that defines the relative position 0004 % and orientation of the piece with respect to the tool at the moment 0005 % when the piece is grabbed. 0006 % 0007 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0008 0009 % Copyright (C) 2012, by Arturo Gil Aparicio 0010 % 0011 % This file is part of ARTE (A Robotics Toolbox for Education). 0012 % 0013 % ARTE is free software: you can redistribute it and/or modify 0014 % it under the terms of the GNU Lesser General Public License as published by 0015 % the Free Software Foundation, either version 3 of the License, or 0016 % (at your option) any later version. 0017 % 0018 % ARTE is distributed in the hope that it will be useful, 0019 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0020 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0021 % GNU Lesser General Public License for more details. 0022 % 0023 % You should have received a copy of the GNU Leser General Public License 0024 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0025 function simulation_release_piece 0026 0027 global configuration robot 0028 0029 %T07=directkinematic(robot, robot.q)*robot.tool.TCP; 0030 %robot.tool.Trel=inv(T07)*robot.piece.T0; 0031 T07=directkinematic(robot, robot.q); 0032 if isfield(robot, 'tool') 0033 T07=T07*robot.tool.TCP; 0034 end 0035 0036 % the piece is now released 0037 robot.tool.piece_gripped=0; 0038 0039 %save the last known pose 0040 robot.piece.T0=T07*(robot.tool.Trel); 0041 0042 0043 0044 0045 0046 0047 0048 0049 0050