select_closest_configuration

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Q=SELECT_CLOSEST_CONFIGURATION(ROBOT, Qinv, CONF)
 Returns the joint coordinates Q that are closest with the axes configuration
 vector CONF, given a set of solutions of the inverse kinematic problem Qinv.
 For 6DOF or less manipulators, the variable CONF={CF1, CF4, CF6, CFX}
 specifies univoquely only one of the solutions. The first axis has
 priority over the rest.

 See also:
   COMPUTE_CONFIGURATION, SELECT_CONFIGURATION , GET_CONF_DATA

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 % Q=SELECT_CLOSEST_CONFIGURATION(ROBOT, Qinv, CONF)
0003 % Returns the joint coordinates Q that are closest with the axes configuration
0004 % vector CONF, given a set of solutions of the inverse kinematic problem Qinv.
0005 % For 6DOF or less manipulators, the variable CONF={CF1, CF4, CF6, CFX}
0006 % specifies univoquely only one of the solutions. The first axis has
0007 % priority over the rest.
0008 %
0009 % See also:
0010 %   COMPUTE_CONFIGURATION, SELECT_CONFIGURATION , GET_CONF_DATA
0011 %
0012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0013 
0014 % Copyright (C) 2012, by Arturo Gil Aparicio
0015 %
0016 % This file is part of ARTE (A Robotics Toolbox for Education).
0017 %
0018 % ARTE is free software: you can redistribute it and/or modify
0019 % it under the terms of the GNU Lesser General Public License as published by
0020 % the Free Software Foundation, either version 3 of the License, or
0021 % (at your option) any later version.
0022 %
0023 % ARTE is distributed in the hope that it will be useful,
0024 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0025 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0026 % GNU Lesser General Public License for more details.
0027 %
0028 % You should have received a copy of the GNU Leser General Public License
0029 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0030 function q=select_closest_configuration(robot, qinv, conf)
0031 
0032 q=qinv(:,1); 
0033 
0034 distance=64;
0035 
0036 for i=1:size(qinv,2),    
0037     confi=compute_configuration(robot, qinv(:,i));
0038     
0039     d = abs(conf(1)-confi(1)) + abs(conf(2)-confi(2)) + abs(conf(3)-confi(3));
0040     
0041     if d < distance
0042        q =  qinv(:,i);
0043        distance=d;       
0044     end
0045 end
0046 
0047 if d ~= 0 
0048     disp('WARNING: RAPID/select_closest_configuration: No solutions complies with the specified configuration ');
0049 end