% Copyright (C) 1993-2013, 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

%%begin

% Forward kinematics is the problem of solving the Cartesian position and 
% orientation of a mechanism given knowledge of the kinematic structure and 
% the joint coordinates.
%
% We will work with a model of the Puma 560 robot
mdl_puma560

% Consider the Puma 560 example again, and the joint coordinates of zero,
% which are defined by the script

qz

% The forward kinematics may be computed using fkine() method of the
% p560 robot object

p560.fkine(qz)

% returns the homogeneous transform corresponding to the last link of the 
% manipulator

% fkine() can also be used with a time sequence of joint coordinates, or 
% trajectory, which is generated by jtraj()

t = [0:.056:2]; 	% generate a time vector
q = jtraj(qz, qr, t); % compute the joint coordinate trajectory

about q

% then the homogeneous transform for each set of joint coordinates is given by

T = p560.fkine(q);

about T

% where T is a 3-dimensional matrix, the first two dimensions are a 4x4 
% homogeneous transformation and the third dimension is time.
%
% For example, the first point is

T(:,:,1)

% and the tenth point is

T(:,:,10)

%
% Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and 
% may be plotted against time

subplot(3,1,1)
plot(t, squeeze(T(1,4,:)))
xlabel('Time (s)');
ylabel('X (m)')
subplot(3,1,2)
plot(t, squeeze(T(2,4,:)))
xlabel('Time (s)');
ylabel('Y (m)')
subplot(3,1,3)
plot(t, squeeze(T(3,4,:)))
xlabel('Time (s)');
ylabel('Z (m)')

% or we could plot X against Z to get some idea of the Cartesian path followed
% by the manipulator.

subplot(1,1,1)
plot(squeeze(T(1,4,:)), squeeze(T(3,4,:)));
xlabel('X (m)')
ylabel('Z (m)')
grid