📄 ikine.html
字号:
<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/REC-html40/loose.dtd"><html><head> <title>Description of ikine</title> <meta name="keywords" content="ikine"> <meta name="description" content="IKINE Inverse manipulator kinematics"> <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1"> <meta name="generator" content="m2html © 2003 Guillaume Flandin"> <meta name="robots" content="index, follow"> <link type="text/css" rel="stylesheet" href="./m2html.css"></head><body><a name="_top"></a><div><a href="./index.html">Home</a> > <a href="index.html">.</a> > ikine.m</div><!--<table width="100%"><tr><td align="left"><a href="./index.html"><img alt="<" border="0" src="./left.png"> Master index</a></td><td align="right"><a href="index.html">Index for . <img alt=">" border="0" src="./right.png"></a></td></tr></table>--><h1>ikine</h1><h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="./up.png"></a></h2><div class="box"><strong>IKINE Inverse manipulator kinematics</strong></div><h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="./up.png"></a></h2><div class="box"><strong>function qt = ikine(robot, tr, q, m) </strong></div><h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="./up.png"></a></h2><div class="fragment"><pre class="comment">IKINE Inverse manipulator kinematics Q = IKINE(ROBOT, T) Q = IKINE(ROBOT, T, Q) Q = IKINE(ROBOT, T, Q, M) Returns the joint coordinates corresponding to the end-effector transform T. Note that the inverse kinematic solution is generally not unique, and depends on the initial guess Q (which defaults to 0). QT = IKINE(ROBOT, TG) QT = IKINE(ROBOT, TG, Q) QT = IKINE(ROBOT, TG, Q, M) Returns the joint coordinates corresponding to each of the transforms in the 4x4xN trajectory TG. Returns one row of QT for each input transform. The initial estimate of QT for each time step is taken as the solution from the previous time step. If the manipulator has fewer than 6 DOF then this method of solution will fail, since the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In such a case it is necessary to provide a mask matrix, M, which specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask matrix 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. Solution is computed iteratively using the pseudo-inverse of the manipulator Jacobian. Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically. This approach allows a solution to obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. For instance with a typical 5 DOF manipulator one would ignore rotation about the wrist axis, that is, M = [1 1 1 1 1 0]. See also: <a href="fkine.html" class="code" title="function t = fkine(robot, q)">FKINE</a>, <a href="tr2diff.html" class="code" title="function d = tr2diff(t1, t2)">TR2DIFF</a>, <a href="jacob0.html" class="code" title="function J0 = jacob0(robot, q)">JACOB0</a>, <a href="ikine560.html" class="code" title="function theta = ikine560(robot, T,configuration)">IKINE560</a>.</pre></div><!-- crossreference --><h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="./up.png"></a></h2>This function calls:<ul style="list-style-image:url(./matlabicon.gif)"><li><a href="fkine.html" class="code" title="function t = fkine(robot, q)">fkine</a> FKINE Forward robot kinematics for serial link manipulator</li><li><a href="ishomog.html" class="code" title="function h = ishomog(tr)">ishomog</a> ISHOMOG Test if argument is a homogeneous transformation</li><li><a href="jacob0.html" class="code" title="function J0 = jacob0(robot, q)">jacob0</a> JACOB0 Compute manipulator Jacobian in world coordinates</li><li><a href="tr2diff.html" class="code" title="function d = tr2diff(t1, t2)">tr2diff</a> TR2DIFF Convert a transform difference to differential representation</li></ul>This function is called by:<ul style="list-style-image:url(./matlabicon.gif)"></ul><!-- crossreference --><h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="./up.png"></a></h2><div class="fragment"><pre>0001 <span class="comment">%IKINE Inverse manipulator kinematics</span>0002 <span class="comment">%</span>0003 <span class="comment">% Q = IKINE(ROBOT, T)</span>0004 <span class="comment">% Q = IKINE(ROBOT, T, Q)</span>0005 <span class="comment">% Q = IKINE(ROBOT, T, Q, M)</span>0006 <span class="comment">%</span>0007 <span class="comment">% Returns the joint coordinates corresponding to the end-effector transform T.</span>0008 <span class="comment">% Note that the inverse kinematic solution is generally not unique, and</span>0009 <span class="comment">% depends on the initial guess Q (which defaults to 0).</span>0010 <span class="comment">%</span>0011 <span class="comment">% QT = IKINE(ROBOT, TG)</span>0012 <span class="comment">% QT = IKINE(ROBOT, TG, Q)</span>0013 <span class="comment">% QT = IKINE(ROBOT, TG, Q, M)</span>0014 <span class="comment">%</span>0015 <span class="comment">% Returns the joint coordinates corresponding to each of the transforms in</span>0016 <span class="comment">% the 4x4xN trajectory TG.</span>0017 <span class="comment">% Returns one row of QT for each input transform. The initial estimate</span>0018 <span class="comment">% of QT for each time step is taken as the solution from the previous</span>0019 <span class="comment">% time step.</span>0020 <span class="comment">%</span>0021 <span class="comment">% If the manipulator has fewer than 6 DOF then this method of solution</span>0022 <span class="comment">% will fail, since the solution space has more dimensions than can</span>0023 <span class="comment">% be spanned by the manipulator joint coordinates. In such a case</span>0024 <span class="comment">% it is necessary to provide a mask matrix, M, which specifies the</span>0025 <span class="comment">% Cartesian DOF (in the wrist coordinate frame) that will be ignored</span>0026 <span class="comment">% in reaching a solution. The mask matrix has six elements that</span>0027 <span class="comment">% correspond to translation in X, Y and Z, and rotation about X, Y and</span>0028 <span class="comment">% Z respectively. The value should be 0 (for ignore) or 1. The number</span>0029 <span class="comment">% of non-zero elements should equal the number of manipulator DOF.</span>0030 <span class="comment">%</span>0031 <span class="comment">% Solution is computed iteratively using the pseudo-inverse of the</span>0032 <span class="comment">% manipulator Jacobian.</span>0033 <span class="comment">%</span>0034 <span class="comment">% Such a solution is completely general, though much less efficient</span>0035 <span class="comment">% than specific inverse kinematic solutions derived symbolically.</span>0036 <span class="comment">%</span>0037 <span class="comment">% This approach allows a solution to obtained at a singularity, but</span>0038 <span class="comment">% the joint angles within the null space are arbitrarily assigned.</span>0039 <span class="comment">%</span>0040 <span class="comment">% For instance with a typical 5 DOF manipulator one would ignore</span>0041 <span class="comment">% rotation about the wrist axis, that is, M = [1 1 1 1 1 0].</span>0042 <span class="comment">%</span>0043 <span class="comment">%</span>0044 <span class="comment">% See also: FKINE, TR2DIFF, JACOB0, IKINE560.</span>0045 0046 <span class="comment">% Copyright (C) 1993-2008, by Peter I. Corke</span>0047 <span class="comment">%</span>0048 <span class="comment">% This file is part of The Robotics Toolbox for Matlab (RTB).</span>0049 <span class="comment">%</span>0050 <span class="comment">% RTB is free software: you can redistribute it and/or modify</span>0051 <span class="comment">% it under the terms of the GNU Lesser General Public License as published by</span>0052 <span class="comment">% the Free Software Foundation, either version 3 of the License, or</span>0053 <span class="comment">% (at your option) any later version.</span>0054 <span class="comment">%</span>0055 <span class="comment">% RTB is distributed in the hope that it will be useful,</span>0056 <span class="comment">% but WITHOUT ANY WARRANTY; without even the implied warranty of</span>0057 <span class="comment">% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the</span>0058 <span class="comment">% GNU Lesser General Public License for more details.</span>0059 <span class="comment">%</span>0060 <span class="comment">% You should have received a copy of the GNU Leser General Public License</span>0061 <span class="comment">% along with RTB. If not, see <http://www.gnu.org/licenses/>.</span>0062 0063 <a name="_sub0" href="#_subfunctions" class="code">function qt = ikine(robot, tr, q, m)</a>0064 <span class="comment">%</span>0065 <span class="comment">% solution control parameters</span>0066 <span class="comment">%</span>0067 ilimit = 1000;0068 stol = 1e-12;0069 0070 n = robot.n;0071 0072 <span class="keyword">if</span> nargin == 2,0073 q = zeros(n, 1);0074 <span class="keyword">else</span>0075 q = q(:);0076 <span class="keyword">end</span>0077 <span class="keyword">if</span> nargin == 4,0078 m = m(:);0079 <span class="keyword">if</span> length(m) ~= 6,0080 error(<span class="string">'Mask matrix should have 6 elements'</span>);0081 <span class="keyword">end</span>0082 <span class="keyword">if</span> length(find(m)) ~= robot.n 0083 error(<span class="string">'Mask matrix must have same number of 1s as robot DOF'</span>)0084 <span class="keyword">end</span>0085 <span class="keyword">else</span>0086 <span class="keyword">if</span> n < 6,0087 disp(<span class="string">'For a manipulator with fewer than 6DOF a mask matrix argument should be specified'</span>);0088 <span class="keyword">end</span>0089 m = ones(6, 1);0090 <span class="keyword">end</span>0091 0092 0093 tcount = 0;0094 <span class="keyword">if</span> <a href="ishomog.html" class="code" title="function h = ishomog(tr)">ishomog</a>(tr), <span class="comment">% single xform case</span>0095 nm = 1;0096 count = 0;0097 <span class="keyword">while</span> nm > stol,0098 e = <a href="tr2diff.html" class="code" title="function d = tr2diff(t1, t2)">tr2diff</a>(<a href="fkine.html" class="code" title="function t = fkine(robot, q)">fkine</a>(robot, q'), tr) .* m;0099 dq = pinv( <a href="jacob0.html" class="code" title="function J0 = jacob0(robot, q)">jacob0</a>(robot, q) ) * e;0100 q = q + dq;0101 nm = norm(dq);0102 count = count+1;0103 <span class="keyword">if</span> count > ilimit,0104 error(<span class="string">'Solution wouldn''t converge'</span>)0105 <span class="keyword">end</span>0106 <span class="keyword">end</span>0107 qt = q';0108 <span class="keyword">else</span> <span class="comment">% trajectory case</span>0109 np = size(tr,3);0110 qt = [];0111 <span class="keyword">for</span> i=1:np0112 nm = 1;0113 T = tr(:,:,i);0114 count = 0;0115 <span class="keyword">while</span> nm > stol,0116 e = <a href="tr2diff.html" class="code" title="function d = tr2diff(t1, t2)">tr2diff</a>(<a href="fkine.html" class="code" title="function t = fkine(robot, q)">fkine</a>(robot, q'), T) .* m;0117 dq = pinv( <a href="jacob0.html" class="code" title="function J0 = jacob0(robot, q)">jacob0</a>(robot, q) ) * e;0118 q = q + dq;0119 nm = norm(dq);0120 count = count+1;0121 <span class="keyword">if</span> count > ilimit,0122 fprintf(<span class="string">'i=%d, nm=%f\n'</span>, i, nm);0123 error(<span class="string">'Solution wouldn''t converge'</span>)0124 <span class="keyword">end</span>0125 <span class="keyword">end</span>0126 qt = [qt; q'];0127 tcount = tcount + count;0128 <span class="keyword">end</span>0129 <span class="keyword">end</span></pre></div><hr><address>Generated on Sun 15-Feb-2009 18:09:29 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> © 2003</address></body></html>⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -