RTFKDEMO Forward kinematics demo
0001 %RTFKDEMO Forward kinematics demo 0002 0003 % Copyright (C) 1993-2002, by Peter I. Corke 0004 0005 % $Log: not supported by cvs2svn $ 0006 % Revision 1.3 2002-04-02 12:26:48 pic 0007 % Handle figures better, control echo at end of each script. 0008 % Fix bug in calling ctraj. 0009 % 0010 % Revision 1.2 2002/04/01 11:47:17 pic 0011 % General cleanup of code: help comments, see also, copyright, remnant dh/dyn 0012 % references, clarification of functions. 0013 % 0014 % $Revision: 1.1 $ 0015 figure(2) 0016 echo on 0017 % Forward kinematics is the problem of solving the Cartesian position and 0018 % orientation of a mechanism given knowledge of the kinematic structure and 0019 % the joint coordinates. 0020 % 0021 % Consider the Puma 560 example again, and the joint coordinates of zero, 0022 % which are defined by qz 0023 qz 0024 % 0025 % The forward kinematics may be computed using fkine() with an appropropriate 0026 % kinematic description, in this case, the matrix p560 which defines 0027 % kinematics for the 6-axis Puma 560. 0028 fkine(p560, qz) 0029 % 0030 % returns the homogeneous transform corresponding to the last link of the 0031 % manipulator 0032 pause % any key to continue 0033 % 0034 % fkine() can also be used with a time sequence of joint coordinates, or 0035 % trajectory, which is generated by jtraj() 0036 % 0037 t = [0:.056:2]; % generate a time vector 0038 q = jtraj(qz, qr, t); % compute the joint coordinate trajectory 0039 % 0040 % then the homogeneous transform for each set of joint coordinates is given by 0041 T = fkine(p560, q); 0042 0043 % 0044 % where T is a 3-dimensional matrix, the first two dimensions are a 4x4 0045 % homogeneous transformation and the third dimension is time. 0046 % 0047 % For example, the first point is 0048 T(:,:,1) 0049 % 0050 % and the tenth point is 0051 T(:,:,10) 0052 pause % any key to continue 0053 % 0054 % Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and 0055 % may be plotted against time 0056 subplot(3,1,1) 0057 plot(t, squeeze(T(1,4,:))) 0058 xlabel('Time (s)'); 0059 ylabel('X (m)') 0060 subplot(3,1,2) 0061 plot(t, squeeze(T(2,4,:))) 0062 xlabel('Time (s)'); 0063 ylabel('Y (m)') 0064 subplot(3,1,3) 0065 plot(t, squeeze(T(3,4,:))) 0066 xlabel('Time (s)'); 0067 ylabel('Z (m)') 0068 pause % any key to continue 0069 % 0070 % or we could plot X against Z to get some idea of the Cartesian path followed 0071 % by the manipulator. 0072 % 0073 subplot(1,1,1) 0074 plot(squeeze(T(1,4,:)), squeeze(T(3,4,:))); 0075 xlabel('X (m)') 0076 ylabel('Z (m)') 0077 grid 0078 pause % any key to continue 0079 echo off