FKINE Forward robot kinematics for serial link manipulator TR = FKINE(ROBOT, Q) Computes the forward kinematics for each joint space point defined by Q. ROBOT is a robot object. For an n-axis manipulator Q is an n element vector or an m x n matrix of robot joint coordinates. If Q is a vector it is interpretted as the generalized joint coordinates, and FKINE(ROBOT, Q) returns a 4x4 homogeneous transformation for the tool of the manipulator. If Q is a matrix, the rows are interpretted as the generalized joint coordinates for a sequence of points along a trajectory. Q(i,j) is the j'th joint parameter for the i'th trajectory point. In this case FKINE(ROBOT, Q) returns 3D matrix where the last subscript is the index along the path. The robot's base or tool transform, if present, are incorporated into the result. See also: LINK, ROBOT.
0001 %FKINE Forward robot kinematics for serial link manipulator 0002 % 0003 % TR = FKINE(ROBOT, Q) 0004 % 0005 % Computes the forward kinematics for each joint space point defined by Q. 0006 % ROBOT is a robot object. 0007 % 0008 % For an n-axis manipulator Q is an n element vector or an m x n matrix of 0009 % robot joint coordinates. 0010 % 0011 % If Q is a vector it is interpretted as the generalized joint coordinates, and 0012 % FKINE(ROBOT, Q) returns a 4x4 homogeneous transformation for the tool of 0013 % the manipulator. 0014 % 0015 % If Q is a matrix, the rows are interpretted as the generalized 0016 % joint coordinates for a sequence of points along a trajectory. Q(i,j) is 0017 % the j'th joint parameter for the i'th trajectory point. In this case 0018 % FKINE(ROBOT, Q) returns 3D matrix where the last subscript is the index 0019 % along the path. 0020 % 0021 % The robot's base or tool transform, if present, are incorporated into the 0022 % result. 0023 % 0024 % See also: LINK, ROBOT. 0025 0026 % Copyright (C) 1993-2008, by Peter I. Corke 0027 % 0028 % This file is part of The Robotics Toolbox for Matlab (RTB). 0029 % 0030 % RTB is free software: you can redistribute it and/or modify 0031 % it under the terms of the GNU Lesser General Public License as published by 0032 % the Free Software Foundation, either version 3 of the License, or 0033 % (at your option) any later version. 0034 % 0035 % RTB is distributed in the hope that it will be useful, 0036 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0037 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0038 % GNU Lesser General Public License for more details. 0039 % 0040 % You should have received a copy of the GNU Leser General Public License 0041 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0042 0043 function t = fkine(robot, q) 0044 % 0045 % evaluate fkine for each point on a trajectory of 0046 % theta_i or q_i data 0047 % 0048 0049 n = robot.n; 0050 0051 L = robot.link; 0052 if length(q) == n, 0053 t = robot.base; 0054 for i=1:n, 0055 t = t * L{i}(q(i)); 0056 end 0057 t = t * robot.tool; 0058 else 0059 if numcols(q) ~= n, 0060 error('bad data') 0061 end 0062 t = zeros(4,4,0); 0063 for qv=q', % for each trajectory point 0064 tt = robot.base; 0065 for i=1:n, 0066 tt = tt * L{i}(qv(i)); 0067 end 0068 t = cat(3, t, tt * robot.tool); 0069 end 0070 end