%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CONF= COMPUTE_CONFIGURATION(Q)
Returns the configuration corresponding to the specified joint coordinates
In RAPID the configuration variable is used to distinguish among the
different solutions for the inverse kinematic problem.
The variable conf=[cf1 cf4 cf6 cfx] is formed by the position of the
first, fourth and sixth robot axis.
If the angle is positive, the conf value has one of the
following values:
1 | 0
---|---
2 | 3
that can be computed with the formulae: floor(joint_angle x 2/pi)
if the angle is negative, then
-3 | -4
---|---
-2 | -1
computed as: ceil(joint_angle × 2/pi - 1)
The last value cfx is necessary for robots that possess eight possible
solutions for the inverse kinematic problem in position/orientation.
Otherwise, specifying only three angles with cf1 cf4 and cf6 would not suffice.
This is the case of the IRB140, the IRB 6650, IRB120, IRB52... and more.
Other robots manufactured by others may have similar ways to define a
particular solution of the inverse kinematic.
For a complete description, see:
http://developercenter.robotstudio.com/BlobProxy/manuals/RapidIFDTechRefManual/doc439.html
The compute configuration function gives different values depending on the robots name
The compute_configuration function calls different internal functions to
give the correct configurations values, depending on the robot model. The
last value cfx is not computed and rather given. This value depends
directly on the inversekinematic function defined for any particular
function. The inversekinematic function returns the different solutions
in a given specified order
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % CONF= COMPUTE_CONFIGURATION(Q) 0003 % Returns the configuration corresponding to the specified joint coordinates 0004 % 0005 % In RAPID the configuration variable is used to distinguish among the 0006 % different solutions for the inverse kinematic problem. 0007 % The variable conf=[cf1 cf4 cf6 cfx] is formed by the position of the 0008 % first, fourth and sixth robot axis. 0009 % 0010 % If the angle is positive, the conf value has one of the 0011 % following values: 0012 % 1 | 0 0013 % ---|--- 0014 % 2 | 3 0015 % that can be computed with the formulae: floor(joint_angle x 2/pi) 0016 % if the angle is negative, then 0017 % -3 | -4 0018 % ---|--- 0019 % -2 | -1 0020 % computed as: ceil(joint_angle × 2/pi - 1) 0021 % 0022 % The last value cfx is necessary for robots that possess eight possible 0023 % solutions for the inverse kinematic problem in position/orientation. 0024 % Otherwise, specifying only three angles with cf1 cf4 and cf6 would not suffice. 0025 % 0026 % This is the case of the IRB140, the IRB 6650, IRB120, IRB52... and more. 0027 % Other robots manufactured by others may have similar ways to define a 0028 % particular solution of the inverse kinematic. 0029 % 0030 % For a complete description, see: 0031 % http://developercenter.robotstudio.com/BlobProxy/manuals/RapidIFDTechRefManual/doc439.html 0032 % 0033 % The compute configuration function gives different values depending on the robots name 0034 % 0035 % The compute_configuration function calls different internal functions to 0036 % give the correct configurations values, depending on the robot model. The 0037 % last value cfx is not computed and rather given. This value depends 0038 % directly on the inversekinematic function defined for any particular 0039 % function. The inversekinematic function returns the different solutions 0040 % in a given specified order 0041 % 0042 % 0043 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0044 0045 % Copyright (C) 2012, by Arturo Gil Aparicio 0046 % 0047 % This file is part of ARTE (A Robotics Toolbox for Education). 0048 % 0049 % ARTE is free software: you can redistribute it and/or modify 0050 % it under the terms of the GNU Lesser General Public License as published by 0051 % the Free Software Foundation, either version 3 of the License, or 0052 % (at your option) any later version. 0053 % 0054 % ARTE is distributed in the hope that it will be useful, 0055 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0056 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0057 % GNU Lesser General Public License for more details. 0058 % 0059 % You should have received a copy of the GNU Leser General Public License 0060 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0061 function conf=compute_configuration(robot, q) 0062 %initialize conf data 0063 conf = zeros(1,4); 0064 0065 switch robot.name 0066 case 'ABB_IRB140_M2000' 0067 conf=compute_configuration_standard_6R(robot, q); 0068 case 'ABB_IRB52' 0069 conf=compute_configuration_standard_6R(robot, q); 0070 case 'ABB_IRB120' 0071 conf=compute_configuration_standard_6R(robot, q); 0072 0073 %if the robot is not found before, compute it as the standard way 0074 otherwise 0075 disp('WARNING: the compute_configuration function did not find a suitable method to compute the configuration'); 0076 disp('WARNING: using a standard function to compute it'); 0077 conf=compute_configuration_standard_6R(robot, q); 0078 end 0079 0080 0081 0082 % Standard way of computing configuration for robots with 6 rotational 0083 % joints. Examples include: 0084 % IRB140, the IRB 6650, IRB120, IRB52 0085 % The cfx is computed as described in: 0086 % 0087 % http://developercenter.robotstudio.com/BlobProxy/manuals/RapidIFDTechRefManual/doc439.html 0088 % cfx | Wrist center relative to axis 1 | Wrist center relative to lower arm | Axis 5 angle 0089 % 0 | In front of | In front of | Positive 0090 % 1 | In front of | In front of | Negative 0091 % 2 | In front of | Behind | Positive 0092 % 3 | In front of | Behind | Negative 0093 % 4 | Behind | In front of | Positive 0094 % 5 | Behind | In front of | Negative 0095 % 6 | Behind | Behind | Positive 0096 % 7 | Behind | Behind | Negative 0097 % 0098 % The table is implemented with three values a, b and c, with the following 0099 % values: 0100 % table_values=[a b c]; 0101 % In front of a=0, Behind a=1 0102 % In front of b=0, Behind b=1 0103 % Positive c=0, Negative c=1 0104 function conf=compute_configuration_standard_6R(robot, q) 0105 0106 if q(1)>=0 0107 cf1 = floor(q(1)*2/pi); 0108 else 0109 cf1 = ceil(q(1)*2/pi - 1); 0110 end 0111 %temp value for conf 0112 conf = [cf1 0 0 0]; 0113 0114 if robot.DOF<4 0115 return; 0116 end 0117 0118 if q(4)>=0 0119 cf4 = floor(q(4)*2/pi); 0120 else 0121 cf4 = ceil(q(4)*2/pi) - 1; 0122 end 0123 %temp value for conf 0124 conf = [cf1 cf4 0 0]; 0125 0126 if robot.DOF<6 0127 return; 0128 end 0129 0130 if q(6)>=0 0131 cf6 = floor(q(6)*2/pi); 0132 else 0133 cf6 = ceil(q(6)*2/pi) - 1; 0134 end 0135 0136 0137 T=directkinematic(robot, q); 0138 0139 table_values=[]; 0140 0141 %given q1 is known, compute first DH transformation 0142 T01=dh(robot, q, 1); 0143 T12=dh(robot, q, 2); 0144 T23=dh(robot, q, 3); 0145 T34=dh(robot, q, 4); 0146 T45=dh(robot, q, 5); 0147 0148 T05=T01*T12*T23*T34*T45; 0149 %compute the wrist point Pw for the robot, given the current joint values q 0150 Pw=T05(1:3,4); 0151 0152 % Express Pw in the reference system 1, for convenience 0153 %pa = inv(T01)*[Pw; 1]; 0154 pa = (T01)\[Pw; 1]; 0155 0156 % The case indicated with Wrist center relative to axis 1 "In front of" 0157 % happens when the X coordinate of pa is positive (value=0). Otherwise, it is 1 and 0158 % indicated as Behind. 0159 if pa(1)>=0 0160 table_values(1)=0; 0161 else 0162 table_values(1)=1; 0163 end 0164 0165 % Express Pw in the reference system 2, for convenience 0166 %pb = inv(T01*T12)*[Pw; 1]; 0167 pb = (T01*T12)\[Pw; 1]; 0168 0169 % The case indicated with Wrist center relative to lower arm "In front of" 0170 % happens when the Y coordinate of pb is positive (value=0). Otherwise, it is 1 and 0171 % indicated as Behind. 0172 if pb(2)>=0 0173 table_values(2)=0; 0174 else 0175 table_values(2)=1; 0176 end 0177 0178 0179 %finally, the last value refers to the sign of q(5), positive=0 0180 %negative=1 0181 if q(5)>=0 0182 table_values(3)=0; 0183 else 0184 table_values(3)=1; 0185 end 0186 0187 0188 %now give the cfx value according to the specified table 0189 if isequal(table_values, [0 0 0]) 0190 cfx=0; 0191 elseif isequal(table_values, [0 0 1]) 0192 cfx=1; 0193 elseif isequal(table_values, [0 1 0]) 0194 cfx=2; 0195 elseif isequal(table_values, [0 1 1]) 0196 cfx=3; 0197 elseif isequal(table_values, [1 0 0]) 0198 cfx=4; 0199 elseif isequal(table_values, [1 0 1]) 0200 cfx=5; 0201 elseif isequal(table_values, [1 1 0]) 0202 cfx=6; 0203 else 0204 cfx=7; 0205 end 0206 0207 conf = [cf1 cf4 cf6 cfx]; 0208