	echo on
% 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.
%
%  Consider the Puma 560 example again, and the joint coordinates of zero, which are
% defined by qz
	qz
%
%  The forward kinematics may be computed using fkine() with an appropropriate kinematic
% description, in this case, the matrix p560 which defines kinematics for the 6-axis
% Puma 560.
	fkine(p560, qz)
%
% returns the homogeneous transform corresponding to the last link of the manipulator
pause	% any key to continue
%
%  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
%
%  then the homogeneous transform for each set of joint coordinates is given by
	T = fkine(p560, q);

%
% where T has one row for each time step, and that row is a `flattened' 4x4 homogeneous
% tranform, which can be restored to its original shape using reshape().  For
% example, the first point is
	reshape(T(1,:), 4,4)
%
% and the tenth point is
	reshape(T(10,:), 4,4)
pause	% any key to continue
%
% Columns 13, 14 and 15 of t correspond to the X, Y and Z coordinates respectively, and 
% may be plotted against time
	subplot(3,1,1)
	plot(t, T(:,13))
	xlabel('Time (s)');
	ylabel('X (m)')
	subplot(3,1,2)
	plot(t, T(:,14))
	xlabel('Time (s)');
	ylabel('Y (m)')
	subplot(3,1,3)
	plot(t, T(:,15))
	xlabel('Time (s)');
	ylabel('Z (m)')
pause	% any key to continue
%
% or we could plot X against Z to get some idea of the Cartesian path followed
% by the manipulator.
%
	subplot(1,1,1)
	plot(T(:,13), T(:,15))
	xlabel('X (m)')
	ylabel('Z (m)')
	grid
pause	% any key to continue
echo off
