%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% index = find_first_in_zone_data(robot, q, T, radius) Returns the index in the vector q that first goes into a sphere of the specified radius around the target specified by T if radius==0, the last index is returned q is a matrix, where each column stores robot.DOF joint coordinates at a particular time. Author: Arturo Gil. Universidad Miguel Hernández de Elche Date: 07/04/2012 See also: test_joints %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % index = find_first_in_zone_data(robot, q, T, radius) 0003 % 0004 % Returns the index in the vector q that first goes into a sphere of 0005 % the specified radius around the target specified by T 0006 % if radius==0, the last index is returned 0007 % q is a matrix, where each column stores robot.DOF joint coordinates at a 0008 % particular time. 0009 % 0010 % Author: Arturo Gil. Universidad Miguel Hernández de Elche 0011 % Date: 07/04/2012 0012 % 0013 % See also: test_joints 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 index = find_first_in_zone_data(robot, q, T, radius) 0033 0034 if radius==0 0035 index=size(q,2); 0036 return; 0037 end 0038 0039 final = T(1:3,4); 0040 0041 for index=1:size(q,2), 0042 Ti = directkinematic(robot, q(:,index)); 0043 current = Ti(1:3,4); 0044 distance = ((final-current)'*(final-current)); 0045 if distance <= radius^2 0046 %return current index 0047 return; 0048 end 0049 end