Matlab Robotics 逆运动学求解函数（ikine）分析_警告: ikine: rejected-step limit 100 exceeded (pose

注：全文为本人见解，如有错误请指点！

一. 源代码的解释

以下代码直接对robotics工具箱中的ikine函数进行解释

%SerialLink.ikine Inverse kinematics by optimization without joint limits
%
% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot
% end-effector pose T which is an SE3 object or homogenenous transform
% matrix (4x4), and N is the number of robot joints.
%
% This method can be used for robots with any number of degrees of freedom.
% 这个方法适用于任意自由度的机器人
%
% Options::
% 'ilimit',L        maximum number of iterations (default 500)      迭代次数
% 'rlimit',L        maximum number of consecutive step rejections (default 100)      最大连续拒绝次数
% 'tol',T           final error tolerance (default 1e-10)   最终误差容限
% 'lambda',L        initial value of lambda (default 0.1)   步长
% 'lambdamin',M     minimum allowable value of lambda (default 0)	步长的最小值
% 'quiet'           be quiet	输出保持安静
% 'verbose'         be verbose		输出内容详细
% 'mask',M          mask vector (6x1) that correspond to translation in X, Y and Z, 
%                   and rotation about X, Y and Z respectively.		mask向量，表示机器人自由度
% 'q0',Q            initial joint configuration (default all zeros)     迭代的初始关节形位
% 'search'          search over all configurations      搜索所有形位
% 'slimit',L        maximum number of search attempts (default 100)     最大搜索尝试数目
% 'transpose',A     use Jacobian transpose with step size A, rather than
%                   Levenberg-Marquadt      使用步长为A的雅可比转置，而不是LM方法
%
% Trajectory operation::
%
% In all cases if T is a vector of SE3 objects (1xM) or a homogeneous
% transform sequence (4x4xM) then returns the joint coordinates
% corresponding to each of the transforms in the sequence.  Q is MxN where
% N is the number of robot joints. The initial estimate of Q for each time
% step is taken as the solution from the previous time step.
% 使用上一步的解作为当前步的初始值
%
% Underactuated robots::    欠驱动机器人
%
% For the case where the manipulator has fewer than 6 DOF the solution
% space has more dimensions than can be spanned by the manipulator joint
% coordinates.
% 欠驱动机器人解空间维数比关节坐标维数多？
%
% In this case we specify the 'mask' option where the mask
% vector (1x6) specifies the Cartesian DOF (in the wrist coordinate
% frame) that will be ignored in reaching a solution.  The mask vector
% has six elements that correspond to translation in X, Y and Z, and rotation
% about X, Y and Z respectively.  The value should be 0 (for ignore) or 1.
% The number of non-zero elements should equal the number of manipulator DOF.
% 使用“mask”矢量指定失去的自由度，机器人自由度等于改矢量的非零元素的个数。
%
% For example when using a 3 DOF manipulator rotation orientation might be
% unimportant in which case use the option: 'mask', [1 1 1 0 0 0].
% 举个例子，当使用三自由度机械手时，旋转方向可能不重要（不影响逆运动学求解），mask向量设置为[1 1 1 0 0 0]
%
% For robots with 4 or 5 DOF this method is very difficult to use since
% orientation is specified by T in world coordinates and the achievable
% orientations are a function of the tool position.
% 对于4或5个自由度的机器人，这种方法较难使用
%
% References::
% - Robotics, Vision & Control, P. Corke, Springer 2011, Section 8.4.
%
% Notes::
% - This has been completely reimplemented in RTB 9.11
% - Does NOT require MATLAB Optimization Toolbox.   不需要matlab优化工具箱
% - Solution is computed iteratively.   采用迭代的方法
% - Implements a Levenberg-Marquadt variable step size solver.
%   通过LM变步长求解器实现
% - The tolerance is computed on the norm of the error between current
%   and desired tool pose.  This norm is computed from distances
%   and angles without any kind of weighting.
%   误差是根据当前工具姿态和期望（求解）姿态的范数计算
% - The inverse kinematic solution is generally not unique, and
%   depends on the initial guess Q0 (defaults to 0).
%   求解的值不唯一，且与初值有关，初值Q0默认为0
% - The default value of Q0 is zero which is a poor choice for most
%   manipulators (eg. puma560, twolink) since it corresponds to a kinematic
%   singularity.
%   Q0设置为[0 0 0 0 0 0]，这对于部机器人（puma560，twolink）不是好选择，因为其对应运动学奇点
% - Such a solution is completely general, though much less efficient
%   than specific inverse kinematic solutions derived symbolically, like
%   ikine6s or ikine3.
% 	这是通用的解法，所以效率不高
% - This approach allows a solution to be obtained at a singularity, but
%   the joint angles within the null space are arbitrarily assigned.
% 	此方法可能在奇异点求出解
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% 	如果定义了关节偏移，将添加到逆运动学中以生成Q
% - Joint limits are not considered in this solution.
%　 不考虑关节的范围限制
% - The 'search' option peforms a brute-force search with initial conditions
%   chosen from the entire configuration space.
% 	“search”选项形成一个强力搜索，初始条件从整个关节空间中随机选择
% - If the 'search' option is used any prismatic joint must have joint
%   limits defined.
% 	如果使用“搜索”选项，则任何移动关节都必须定义关节范围
%
% See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.fkine, SerialLink.ikine6s.



% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
% 
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
% 
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
% 
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%TODO:
% search do a broad search from random points in configuration space

function qt = ikine(robot, tr, varargin)
    
    n = robot.n;        	%关节变量的个数
    
    TT = SE3.check(tr);     %转换为SE3矩阵
        
    %  set default parameters for solution   默认参数设置
    opt.ilimit = 500;   	%默认迭代次数
    opt.rlimit = 100;   	%最大连续拒绝次数
    opt.slimit = 100;   	%最大尝试次数
    opt.tol = 1e-10;    	%默认迭代误差
    opt.lambda = 0.1;  		%默认步长
    opt.lambdamin = 0;  	%默认步长最小值
    opt.search = false; 	%默认关闭搜索初值
    opt.quiet = false;  	%保持安静，减少输出
    opt.verbose = false;        %输出的信息详细
    opt.mask = [1 1 1 1 1 1];   %默认六自由度
    opt.q0 = zeros(1, n);   	%初始关节形位为零位
    opt.transpose = NaN;    	%默认使用LM方法
    
    [opt,args] = tb_optparse(opt, varargin);	%输入参数传递
    
    if opt.search    				%如果为true，执行，随机选取迭代初值
        % randomised search for a starting point
        
        opt.search = false;         %关闭搜索初值
        opt.quiet = true;           %保持安静
        %args = args{2:end};
        
        for k=1:opt.slimit          %循环到最大尝试次数
            for j=1:n
                qlim = robot.links(j).qlim;     %传递机器人关节变量的范围
                if isempty(qlim)    			%是否为空集
                    if robot.links(j).isrevolute    %是否为旋转关节
                        q(j) = rand*2*pi - pi;      %产生-pi到pi的随机初值
                    else
                        error('For a prismatic joint, search requires joint limits');   %对于移动关节，需要定义关节范围
                    end
                else
                    q(j) = rand*(qlim(2)-qlim(1)) + qlim(1);    %产生关节范围内的随机初值
                end
            end
            fprintf('Trying q = %s\n', num2str(q));     %输出“尝试q的值为 ”
            
            q = robot.ikine(tr, q, args{:}, 'setopt', opt);     %以此为初值，计算结果
            if ~isempty(q)
                qt = q;									%如果不为空集，q为所求值
                return;
            end
        end
        error('no solution found, are you sure the point is reachable?');   %找不到解，你确定该点可到达？（尝试完最大尝试次数）
        qt = [];
        return
    end
    
    assert(numel(opt.mask) == 6, 'RTB:ikine:badarg', 'Mask matrix should have 6 elements');     %如果mask没有6个参数，报错
    assert(n >= numel(find(opt.mask)), 'RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix');      %如果机器人自由度小于mask矩阵的1元素个数，报错
    W = diag(opt.mask);     %W为以mask矩阵为对角线的矩阵
    
    
    qt = zeros(length(TT), n);  % preallocate space for results   为解预分配空间
    tcount = 0;              % total iteration count    初始化总迭代次数
    rejcount = 0;            % rejected step count      初始化持续拒绝的迭代次数
    
    q = opt.q0;     %给迭代初值
    
    failed = false;                     %迭代失败的标签
    revolutes = robot.isrevolute();     %给机器人的关节构型（移动为0或旋转为1，全为旋转为[1 1 1 1 1 1]）
    
    for i=1:length(TT)                  %循环到输入的SE3的个数
        T = TT(i);                      %给其中某个SE3
        lambda = opt.lambda;            %给默认步长

        iterations = 0;                 %初始化迭代次数
        
        if opt.debug                    %是否调试程序，默认不调试，跳 if
            e = tr2delta(robot.fkine(q), T);    %给从初始零位的位姿到输入位姿的对应微分运动，d=(dx, dy, dz, dRx, dRy, dRz)
            fprintf('Initial:  |e|=%g\n', norm(W*e));
        end
        
        while true                              %开始进行迭代
            % update the count and test against iteration limit
            iterations = iterations + 1;        %计数迭代次数
            if iterations > opt.ilimit          %超过迭代最大次数
                if ~opt.quiet                   %quiet值为0，不保持安静，执行
                    warning('ikine: iteration limit %d exceeded (pose %d), final err %g', ...
                        opt.ilimit, i, nm);
                end
                failed = true;
                break                           %迭代失败，并跳出循环
            end
            
            e = tr2delta(robot.fkine(q), T);
            
            % are we there yet
            if norm(W*e) < opt.tol
                break;                          %如果小于容许误差，跳出循环
            end
            
            % compute the Jacobian
            J = jacobe(robot, q);               %给当前形位下的雅各比矩阵
            
            JtJ = J'*W*J;
            
            if ~isnan(opt.transpose)            %是否使用LM方法，进行
                % do the simple Jacobian transpose with constant gain
                dq = opt.transpose * J' * e;
            else
                % do the damped inverse Gauss-Newton with Levenberg-Marquadt
                % 用LM法做阻尼逆高斯牛顿
                dq = inv(JtJ + (lambda + opt.lambdamin) * eye(size(JtJ)) ) * J' * W * e;        %迭代公式
                
                % compute possible new value of   得到新的关节值
                qnew = q + dq';
                
                % and figure out the new error    得到新的误差值
                enew = tr2delta(robot.fkine(qnew), T);
                
                % was it a good update?
                if norm(W*enew) < norm(W*e)			%该步是否成功
                    % step is accepted		
                    if opt.debug                %是否调试程序，默认不调试，跳 if
                        fprintf('ACCEPTED: |e|=%g, |dq|=%g, lambda=%g\n', norm(W*enew), norm(dq), lambda);
                    end
                    q = qnew;
                    e = enew;					%给迭代新值
                    lambda = lambda/2;
                    rejcount = 0;               %持续拒绝的迭代次数置0
                else
                    % step is rejected, increase the damping and retry
                    % 该步被拒绝，增加阻尼并重试
                    if opt.debug                %是否调试程序，默认不调试，跳 if
                        fprintf('rejected: |e|=%g, |dq|=%g, lambda=%g\n', norm(W*enew), norm(dq), lambda);
                    end
                    lambda = lambda*2;
                    rejcount = rejcount + 1;
                    if rejcount > opt.rlimit        %超过最大拒绝次数，进行
                        if ~opt.quiet				%是否保持输出安静，默认保持，跳 else
                            warning('ikine: rejected-step limit %d exceeded (pose %d), final err %g', ...
                                opt.rlimit, i, norm(W*enew));
                        end
                        failed = true;
                        break;                      %迭代失败，跳出循环
                    end
                    continue;                       % try again   修改步长后继续尝试
                end
            end
            
            
            % wrap angles for revolute joints		将旋转关节的关节值移到-pi~pi中
            k = (q > pi) & revolutes;
            q(k) = q(k) - 2*pi;
            
            k = (q < -pi) & revolutes;
            q(k) = q(k) + 2*pi;
            
            nm = norm(W*e);
            
            
        end  % end ikine solution for this pose
        qt(i,:) = q';
        tcount = tcount + iterations;             %总共迭代次数，包括多个SE3值的情况
        if opt.verbose && ~failed                 %是否保持输出详细且迭代成功
            fprintf('%d iterations\n', iterations);
        end
        if failed
            if ~opt.quiet                         %是否失败且不保持安静
                warning('failed to converge: try a different initial value of joint coordinates');      %未能收敛，尝试不同的初值
            end
            qt = [];
        end
    end
    
    
    if opt.verbose && length(TT) > 1              %如果详细且有多个SE3值
        fprintf('TOTAL %d iterations\n', tcount);       %输出总迭代次数
    end      
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301

二. 一些解释

1. 设置的”search“选项不起作用
   我经过多次尝试，发现无论其随机选取的迭代初值q为多少，程序实际上还是从q0=[0 0 0 0 0 0]开始迭代。
   我猜测，q = robot.ikine(tr, q, args{:}, 'setopt', opt);这行代码有问题。
   随机选取的迭代初值q，并没有被传递进去。
   将这行代码改为 q = robot.ikine(tr, 'setopt', opt, 'q0',q, args{:}); 有效果，但不知是否有其他影响，请慎重

2. 迭代公式
   未完待续~