%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q=SELECT_CONFIGURATION(ROBOT, Qinv, CONF)
Returns the joint coordinates Q that comply 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.
See also:
COMPUTE_CONFIGURATION, GET_CONF_DATA
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q=SELECT_CONFIGURATION(ROBOT, Qinv, CONF) 0003 % Returns the joint coordinates Q that comply 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. 0007 % 0008 % See also: 0009 % COMPUTE_CONFIGURATION, GET_CONF_DATA 0010 % 0011 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0012 0013 % Copyright (C) 2012, by Arturo Gil Aparicio 0014 % 0015 % This file is part of ARTE (A Robotics Toolbox for Education). 0016 % 0017 % ARTE is free software: you can redistribute it and/or modify 0018 % it under the terms of the GNU Lesser General Public License as published by 0019 % the Free Software Foundation, either version 3 of the License, or 0020 % (at your option) any later version. 0021 % 0022 % ARTE is distributed in the hope that it will be useful, 0023 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0024 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0025 % GNU Lesser General Public License for more details. 0026 % 0027 % You should have received a copy of the GNU Leser General Public License 0028 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0029 function q=select_configuration(robot, qinv, conf) 0030 0031 q=qinv(:,1); %zeros(robot.DOF,1); 0032 0033 for i=1:size(qinv,2), 0034 confi=compute_configuration(robot, qinv(:,i)) 0035 if isequal(conf(1:4), confi(1:4)) 0036 %if the same configuration is found, store the joint values and 0037 %return 0038 q = qinv(:,i); 0039 return; 0040 end 0041 end 0042 0043 disp('ERROR: RAPID/select_configuration: No solutions complies with the exact specified configuration '); 0044 0045 disp('WARNING: Selecting now the closest configuration'); 0046 0047 q=select_closest_configuration(robot, qinv, conf); 0048 0049