%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Q=SELECT_CLOSEST_JOINT_COORDINATES(ROBOT, Qinv, Qcurrent) Given a matrix Qinv, where each column corresponds to a different solution to the inverse kinematic problem. The function returns Q that are closest to Qcurrent See also: COMPUTE_CONFIGURATION, SELECT_CONFIGURATION , GET_CONF_DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % Q=SELECT_CLOSEST_JOINT_COORDINATES(ROBOT, Qinv, Qcurrent) 0003 % Given a matrix Qinv, where each column corresponds to a different 0004 % solution to the inverse kinematic problem. The function returns 0005 % Q that are closest to Qcurrent 0006 % 0007 % See also: 0008 % COMPUTE_CONFIGURATION, SELECT_CONFIGURATION , GET_CONF_DATA 0009 % 0010 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0011 0012 % Copyright (C) 2012, by Arturo Gil Aparicio 0013 % 0014 % This file is part of ARTE (A Robotics Toolbox for Education). 0015 % 0016 % ARTE is free software: you can redistribute it and/or modify 0017 % it under the terms of the GNU Lesser General Public License as published by 0018 % the Free Software Foundation, either version 3 of the License, or 0019 % (at your option) any later version. 0020 % 0021 % ARTE is distributed in the hope that it will be useful, 0022 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0023 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0024 % GNU Lesser General Public License for more details. 0025 % 0026 % You should have received a copy of the GNU Leser General Public License 0027 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0028 function q=select_closest_joint_coordinates(qinv, q_current) 0029 0030 a = qinv-repmat(q_current(:), 1, size(qinv,2)); 0031 a=sum(abs(a)); 0032 [val,i]=min(a); 0033 0034 q=qinv(:,i); 0035