%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [joint, time] = test_joint_limits(robot) Test whether any of the joint angles exceeds the mechanical limits imposed by manufacturer. The planned movement in joint coordinates must be stored in the variable robot.q_vector, where each row stores the trajectory corresponding to each of the joint coordinates. The function returns the joint that caused the fault and the time when it occurred. Author: Arturo Gil. Universidad Miguel Hernández de Elche Date: 05/05/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % [joint, time] = test_joint_limits(robot) 0003 % 0004 % Test whether any of the joint angles exceeds the mechanical 0005 % limits imposed by manufacturer. 0006 % 0007 % The planned movement in joint coordinates must be stored in the 0008 % variable robot.q_vector, where each row stores the trajectory 0009 % corresponding to each of the joint coordinates. 0010 % The function returns the joint that caused the fault and the 0011 % time when it occurred. 0012 % 0013 % Author: Arturo Gil. Universidad Miguel Hernández de Elche 0014 % Date: 05/05/2012 0015 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0016 0017 % Copyright (C) 2012, by Arturo Gil Aparicio 0018 % 0019 % This file is part of ARTE (A Robotics Toolbox for Education). 0020 % 0021 % ARTE is free software: you can redistribute it and/or modify 0022 % it under the terms of the GNU Lesser General Public License as published by 0023 % the Free Software Foundation, either version 3 of the License, or 0024 % (at your option) any later version. 0025 % 0026 % ARTE is distributed in the hope that it will be useful, 0027 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0028 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0029 % GNU Lesser General Public License for more details. 0030 % 0031 % You should have received a copy of the GNU Leser General Public License 0032 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0033 function [joint, time] = test_joint_limits(robot) 0034 0035 0036 %if exist('robot.maxangle', 'var') 0037 if ~isempty(robot.maxangle) 0038 for i=1:robot.DOF, 0039 [index1]=find(robot.q_vector(i,:) < robot.maxangle(i,1)); 0040 [index2]=find(robot.q_vector(i,:) > robot.maxangle(i,2)); 0041 0042 if ~isempty(index1) 0043 fprintf('\nERROR: joint out of range'); 0044 fprintf('\nJoint %d exceeds %f (rad)', i, robot.maxangle(i,2)); 0045 end 0046 if ~isempty(index2) 0047 fprintf('\nERROR: joint out of range'); 0048 fprintf('\nJoint %d below %f (rad)', i, robot.maxangle(i,1)); 0049 end 0050 end 0051 else 0052 disp('WARNING:test_joint_limits: robot.maxangle undefined'); 0053 end 0054