%
% This script file tests the functions of the Chapter 3 Progamming
% Exercises.  The user enters the three joint coordinates, and function
% "where" is used to find the pose of the {TOOL} frame relative to the
% {STATION} frame.  The "tool offset" TrelW and the "station frame offset"
% SrelB are coded in the routine.  The {STATION} frame would more properly
% called a {WORKCELL} frame.  Note that the Chapter 2 functions "utoi" and
% "itou" are also used here.

RAD = pi/180;   % Conversion factor from DEG -> RAD.

% First define the {TOOL} and {STATION} frame offsets:

TrelW_u = [0.1 0.2 30]';    % {TOOL} relative to {WRIST} (tool offset)
SrelB_u = [-0.1 0.3 0]';    % {STATION} relative to {BASE} (workcell offset)

% Next convert the above "user" forms to "internal" forms:

TrelW = utoi(TrelW_u);
SrelB = utoi(SrelB_u);

% Now prompt the user to enter the three joint angles, as a vector which
% must be enclosed in brackets [th1 th2 th3]:

theta = input('Enter joint angle vector in degrees, like [th1 th2 th3]: ');
theta = theta*RAD;  % Convert to RAD.

% Now use the "where()" function to compute TrelS (internal form):

TrelS = where(theta,TrelW,SrelB);

% Finally convert to user form and display the result:

TrelS_u = itou(TrelS)